thomas/Cobots
2
1
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity

Robot actually moves actual real hardware now

Browse source 
Actually

Gear ratios subject to change

Need to move state publisher into ar3_hardware/joint_state_broadcaster
This commit is contained in:
Thomas Muller 2021-09-09 13:38:30 +00:00
parent 70568e30b1
commit 0528093f41
5 changed files with 228 additions and 86 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

12
src/ar3_config/launch/dev.launch.py
View file

@ -111,10 +111,10 @@ def generate_launch_description():
parameters=[robot_description]),
# Fake robot state publisher
Node(
package='ar3_description',
executable='state_publisher',
name='state_publisher',
emulate_tty=True,
output='screen'),
# Node(
# package='ar3_description',
# executable='state_publisher',
# name='state_publisher',
# emulate_tty=True,
# output='screen'),
])

140
src/ar3_config/rviz/ar3.rviz
View file

@ -6,9 +6,8 @@ Panels:
Expanded:
- /Global Options1
- /Status1
- /RobotModel1
Splitter Ratio: 0.5
Tree Height: 616
Tree Height: 218
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
@ -43,18 +42,33 @@ Visualization Manager:
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Class: rviz_default_plugins/RobotModel
Collision Enabled: false
Description File: ""
Description Source: Topic
Description Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /robot_description
- Acceleration_Scaling_Factor: 0.1
Class: moveit_rviz_plugin/MotionPlanning
Enabled: true
Move Group Namespace: ""
MoveIt_Allow_Approximate_IK: false
MoveIt_Allow_External_Program: false
MoveIt_Allow_Replanning: false
MoveIt_Allow_Sensor_Positioning: false
MoveIt_Planning_Attempts: 10
MoveIt_Planning_Time: 5
MoveIt_Use_Cartesian_Path: false
MoveIt_Use_Constraint_Aware_IK: false
MoveIt_Warehouse_Host: 127.0.0.1
MoveIt_Warehouse_Port: 33829
MoveIt_Workspace:
Center:
X: 0
Y: 0
Z: 0
Size:
X: 2
Y: 2
Z: 2
Name: MotionPlanning
Planned Path:
Color Enabled: false
Interrupt Display: false
Links:
All Links Enabled: true
Expand Joint Details: false
@ -96,11 +110,91 @@ Visualization Manager:
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel
TF Prefix: ""
Update Interval: 0
Loop Animation: false
Robot Alpha: 0.5
Robot Color: 150; 50; 150
Show Robot Collision: false
Show Robot Visual: true
Show Trail: false
State Display Time: 0.05 s
Trail Step Size: 1
Trajectory Topic: /display_planned_path
Planning Metrics:
Payload: 1
Show Joint Torques: false
Show Manipulability: false
Show Manipulability Index: false
Show Weight Limit: false
TextHeight: 0.07999999821186066
Planning Request:
Colliding Link Color: 255; 0; 0
Goal State Alpha: 1
Goal State Color: 250; 128; 0
Interactive Marker Size: 0
Joint Violation Color: 255; 0; 255
Planning Group: ar3_arm
Query Goal State: true
Query Start State: false
Show Workspace: false
Start State Alpha: 1
Start State Color: 0; 255; 0
Planning Scene Topic: /monitored_planning_scene
Robot Description: robot_description
Scene Geometry:
Scene Alpha: 0.8999999761581421
Scene Color: 50; 230; 50
Scene Display Time: 0.009999999776482582
Show Scene Geometry: true
Voxel Coloring: Z-Axis
Voxel Rendering: Occupied Voxels
Scene Robot:
Attached Body Color: 150; 50; 150
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Visual Enabled: true
link_1:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
link_2:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
link_3:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
link_4:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
link_5:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
link_6:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Robot Alpha: 1
Show Robot Collision: false
Show Robot Visual: true
Value: true
Velocity_Scaling_Factor: 0.1
Enabled: true
Global Options:
Background Color: 48; 48; 48
@ -144,7 +238,7 @@ Visualization Manager:
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 10
Distance: 2.1567115783691406
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
@ -165,12 +259,16 @@ Visualization Manager:
Yaw: 0.785398006439209
Saved: ~
Window Geometry:
"":
collapsed: false
" - Trajectory Slider":
collapsed: false
Displays:
collapsed: false
Height: 846
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd000000040000000000000156000002f3fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e000002f3000000cb00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003e000002f3000000a500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000023f000002f300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
QMainWindow State: 000000ff00000000fd0000000400000000000001e5000002f3fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e00000165000000cb00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffbffffffff01000001a9000001880000017d00ffffff000000010000010f000002f3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003e000002f3000000a500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000001b0000002f300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Tool Properties:
@ -178,5 +276,5 @@ Window Geometry:
Views:
collapsed: false
Width: 1200
X: 1068
Y: 195
X: 582
Y: 35

12
src/ar3_hardware/src/ar3_system.cpp
View file

@ -118,12 +118,12 @@ hardware_interface::return_type Ar3SystemHardware::start()
};
thread_executor_spin_ = std::thread(spin);
j1_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/net_robot/drives/j1", 10);
j2_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/net_robot/drives/j2", 10);
j3_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/net_robot/drives/j3", 10);
j4_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/net_robot/drives/j4", 10);
j5_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/net_robot/drives/j5", 10);
j6_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/net_robot/drives/j6", 10);
j1_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j1", 10);
j2_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j2", 10);
j3_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j3", 10);
j4_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j4", 10);
j5_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j5", 10);
j6_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j6", 10);
status_ = hardware_interface::status::STARTED;

62
src/cobot_platform/cobot_platform/move.py
View file

@ -1,5 +1,10 @@
import rclpy
import rclpy, re
from rclpy.node import Node
from rclpy.qos import QoSProfile
from sensor_msgs.msg import JointState
from math import pi
from std_msgs.msg import Float32
@ -20,6 +25,7 @@ class CobotMove(Node):
self._serial = Serial('/dev/ttyACM0', 115200)
self._cur = [0.0] * 6
# self._buf = [90, 90, 90] # 3 servos
# self._prev_buf = [90, 90, 90]
@ -28,24 +34,62 @@ class CobotMove(Node):
self.create_subscription(Float32, base_topic+'j1', self._move_j1_cb, rclpy.qos.qos_profile_sensor_data)
self.create_subscription(Float32, base_topic+'j2', self._move_j2_cb, rclpy.qos.qos_profile_sensor_data)
self.create_subscription(Float32, base_topic+'j3', self._move_j3_cb, rclpy.qos.qos_profile_sensor_data)
self.create_subscription(Float32, base_topic+'j4', self._move_j4_cb, rclpy.qos.qos_profile_sensor_data)
self.create_subscription(Float32, base_topic+'j5', self._move_j5_cb, rclpy.qos.qos_profile_sensor_data)
self.create_subscription(Float32, base_topic+'j6', self._move_j6_cb, rclpy.qos.qos_profile_sensor_data)
qos = QoSProfile(depth=10)
self._joint_pub = self.create_publisher(JointState, 'joint_states', qos)
self._joint_state = JointState()
self._serial.write(b'STA0.0.1\n')
print(self._serial.readline())
def _move_j1_cb(self, msg):
self._move_servo(Servos.J1, msg.data)
self._cur[0] = int(msg.data / pi / 2. * 2000)
self._move_servo()
def _move_j2_cb(self, msg):
self._move_servo(Servos.J2, msg.data)
self._cur[1] = int(msg.data / pi / 2. * 3840.64)
self._move_servo()
def _move_j3_cb(self, msg):
self._move_servo(Servos.J3, msg.data)
self._cur[2] = int(msg.data / pi / 2. * 10000)
self._move_servo()
def _move_servo(self, serv_num, pos):
cmd = 'MT'
# self._buf[serv_num] = pos
self._serial.write(f'{cmd}A{int(pos * 10000)}B0C0D0E0F0\n'.encode('UTF-8'))
print(self._serial.readline())
def _move_j4_cb(self, msg):
self._cur[3] = int(msg.data / pi / 2. * 2000)
self._move_servo()
def _move_j5_cb(self, msg):
self._cur[4] = int(msg.data / pi / 2. * 2000)
self._move_servo()
def _move_j6_cb(self, msg):
self._cur[5] = int(msg.data / pi / 2. * 2000)
self._move_servo()
def _move_servo(self):
self._serial.write(f'MTA{self._cur[0]}B{self._cur[1]}C{self._cur[2]}D{self._cur[3]}E{self._cur[4]}F{self._cur[5]}\n'.encode('UTF-8'))
line = self._serial.readline().decode('UTF-8')
blocks = re.findall(r'[ABCDEF]-*\d+', line)
a = float(blocks[0][1:]) / 2000. * pi * 2.
b = float(blocks[1][1:]) / 3840.64 * pi * 2.
c = float(blocks[2][1:]) / 10000. * pi * 2.
d = float(blocks[3][1:]) / 2000. * pi * 2.
e = float(blocks[4][1:]) / 2000. * pi * 2.
f = float(blocks[5][1:]) / 2000. * pi * 2.
self._joint_state.header.stamp = self.get_clock().now().to_msg()
self._joint_state.name = ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6']
self._joint_state.position = [a, b, c, d, e, f]
self._joint_pub.publish(self._joint_state)
print(self._joint_state.position)
# def _push ?

4
src/joystickmove/joystickmove/move.py
View file

@ -12,10 +12,10 @@ class JoyMove(Node):
self.create_subscription(Joy, '/joy', self._joy_cb, rclpy.qos.qos_profile_sensor_data)
self.base_pub = self.create_publisher(Float32, '/move/body', 10)
self.base_pub = self.create_publisher(Float32, '/move/j1', 10)
def _joy_cb(self, msg):
asd = (msg.axes[1] + 1) / 2 * 180.0
asd = msg.axes[1]
self.base_pub.publish(Float32(data=asd))