Skip to content
Snippets Groups Projects
Commit 6ee5c11f authored by Alexander Bubeck's avatar Alexander Bubeck
Browse files

Merge pull request #138 from ipa-fxm/bring_hydro_updates_to_indigo 

[Indigo] Bring hydro updates to indigo
parents 682de4dd 42f70daf
Branches
Tags
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 435 additions and 24 deletions
README.md
+ 3
− 3
View file @ 6ee5c11f
......@@ -17,7 +17,7 @@ __Usage with real Hardware__
There are launch files available to bringup a real robot - either UR5 or UR10.
In the following the commands for the UR5 are given. For the UR10, simply replace the prefix accordingly.
Don't forget to source the correcct setup shell files and use a new terminal for each command!
Don't forget to source the correct setup shell files and use a new terminal for each command!
To bring up the real robot, run:
......@@ -29,7 +29,7 @@ A simple test script that moves the robot to predefined positions can be execute
CAUTION:
Remember that you should always have your hands on the big red button in case there is something in the way or anything unexcpected happens.
Remember that you should always have your hands on the big red button in case there is something in the way or anything unexpected happens.
__MoveIt! with real Hardware__
......@@ -61,7 +61,7 @@ __Usage with Gazebo Simulation__
There are launch files available to bringup a simulated robot - either UR5 or UR10.
In the following the commands for the UR5 are given. For the UR10, simply replace the prefix accordingly.
Don't forget to source the correcct setup shell files and use a new terminal for each command!
Don't forget to source the correct setup shell files and use a new terminal for each command!
To bring up the simulated robot in Gazebo, run:
......
ur_bringup/launch/ur_common.launch
+ 4
− 0
View file @ 6ee5c11f
......@@ -16,6 +16,9 @@
<arg name="min_payload"/>
<arg name="max_payload"/>
<!-- The max_velocity parameter is only used for debugging in the ur_driver. It's not related to actual velocity limits -->
<arg name="max_velocity" default="10.0"/> <!-- [rad/s] -->
<!-- copy the specified IP address to be consistant with ROS-Industrial spec.
NOTE: The ip address is actually passed to the driver on the command line -->
<param name="/robot_ip_address" type="str" value="$(arg robot_ip)"/>
......@@ -27,6 +30,7 @@
<node name="ur_driver" pkg="ur_driver" type="driver.py" args="$(arg robot_ip) $(arg reverse_port)" output="screen">
<param name="min_payload" type="double" value="$(arg min_payload)"/>
<param name="max_payload" type="double" value="$(arg max_payload)"/>
<param name="max_velocity" type="double" value="$(arg max_velocity)"/>
</node>
<!-- TF Buffer Server -->
......
ur_description/urdf/ur10.urdf.xacro
+ 21
− 0
View file @ 6ee5c11f
......@@ -70,6 +70,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur10/visual/Base.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......@@ -100,6 +103,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur10/visual/Shoulder.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......@@ -130,6 +136,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur10/visual/UpperArm.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......@@ -160,6 +169,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur10/visual/Forearm.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......@@ -190,6 +202,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur10/visual/Wrist1.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......@@ -220,6 +235,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur10/visual/Wrist2.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......@@ -250,6 +268,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur10/visual/Wrist3.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......
ur_description/urdf/ur10_joint_limited_robot.urdf
+ 21
− 0
View file @ 6ee5c11f
......@@ -32,6 +32,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur10/visual/Base.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......@@ -57,6 +60,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur10/visual/Shoulder.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......@@ -82,6 +88,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur10/visual/UpperArm.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......@@ -107,6 +116,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur10/visual/Forearm.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......@@ -132,6 +144,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur10/visual/Wrist1.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......@@ -157,6 +172,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur10/visual/Wrist2.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......@@ -182,6 +200,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur10/visual/Wrist3.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......
ur_description/urdf/ur10_robot.urdf
+ 21
− 0
View file @ 6ee5c11f
......@@ -32,6 +32,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur10/visual/Base.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......@@ -57,6 +60,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur10/visual/Shoulder.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......@@ -82,6 +88,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur10/visual/UpperArm.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......@@ -107,6 +116,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur10/visual/Forearm.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......@@ -132,6 +144,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur10/visual/Wrist1.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......@@ -157,6 +172,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur10/visual/Wrist2.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......@@ -182,6 +200,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur10/visual/Wrist3.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......
ur_description/urdf/ur5.urdf.xacro
+ 21
− 0
View file @ 6ee5c11f
......@@ -85,6 +85,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur5/visual/Base.dae" />
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......@@ -115,6 +118,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur5/visual/Shoulder.dae" />
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......@@ -145,6 +151,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur5/visual/UpperArm.dae" />
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......@@ -175,6 +184,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur5/visual/Forearm.dae" />
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......@@ -205,6 +217,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur5/visual/Wrist1.dae" />
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......@@ -235,6 +250,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur5/visual/Wrist2.dae" />
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......@@ -265,6 +283,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur5/visual/Wrist3.dae" />
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......
ur_description/urdf/ur5_joint_limited_robot.urdf
+ 21
− 0
View file @ 6ee5c11f
......@@ -32,6 +32,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur5/visual/Base.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......@@ -57,6 +60,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur5/visual/Shoulder.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......@@ -82,6 +88,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur5/visual/UpperArm.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......@@ -107,6 +116,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur5/visual/Forearm.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......@@ -132,6 +144,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur5/visual/Wrist1.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......@@ -157,6 +172,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur5/visual/Wrist2.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......@@ -182,6 +200,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur5/visual/Wrist3.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......
ur_description/urdf/ur5_robot.urdf
+ 21
− 0
View file @ 6ee5c11f
......@@ -32,6 +32,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur5/visual/Base.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......@@ -57,6 +60,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur5/visual/Shoulder.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......@@ -82,6 +88,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur5/visual/UpperArm.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......@@ -107,6 +116,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur5/visual/Forearm.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......@@ -132,6 +144,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur5/visual/Wrist1.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......@@ -157,6 +172,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur5/visual/Wrist2.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......@@ -182,6 +200,9 @@
<geometry>
<mesh filename="package://ur_description/meshes/ur5/visual/Wrist3.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
......
ur_driver/src/ur_driver/deserialize.py 100644 → 100755
+ 107
− 19
View file @ 6ee5c11f
......@@ -50,7 +50,7 @@ class ToolMode(object):
BOOTLOADER = 249
RUNNING = 253
IDLE = 255
class MasterSafetyState(object):
UNDEFINED = 0
BOOTLOADER = 1
......@@ -68,14 +68,29 @@ class MasterOnOffState(object):
ON = 2
TURNING_OFF = 3
#this class handles different protocol versions
class RobotModeData(object):
@staticmethod
def unpack(buf):
rmd = RobotModeData()
(plen, ptype) = struct.unpack_from("!IB", buf)
if plen == 29:
return RobotModeData_V18.unpack(buf)
elif plen == 38:
return RobotModeData_V30.unpack(buf)
else:
print "RobotModeData has wrong length: " + str(plen)
return rmd
#this parses RobotModeData for versions <= v1.8 (i.e. 1.6, 1.7, 1.8)
class RobotModeData_V18(object):
__slots__ = ['timestamp', 'robot_connected', 'real_robot_enabled',
'power_on_robot', 'emergency_stopped',
'security_stopped', 'program_running', 'program_paused',
'robot_mode', 'speed_fraction']
@staticmethod
def unpack(buf):
rmd = RobotModeData()
rmd = RobotModeData_V18()
(_, _,
rmd.timestamp, rmd.robot_connected, rmd.real_robot_enabled,
rmd.power_on_robot, rmd.emergency_stopped, rmd.security_stopped,
......@@ -83,7 +98,24 @@ class RobotModeData(object):
rmd.speed_fraction) = struct.unpack_from("!IBQ???????Bd", buf)
return rmd
# Don't use T_micro (obsolete). For retrocompatibility purposes.
#this parses RobotModeData for versions >=3.0 (i.e. 3.0)
class RobotModeData_V30(object):
__slots__ = ['timestamp', 'robot_connected', 'real_robot_enabled',
'power_on_robot', 'emergency_stopped',
'security_stopped', 'program_running', 'program_paused',
'robot_mode', 'control_mode', 'target_speed_fraction',
'speed_scaling']
@staticmethod
def unpack(buf):
rmd = RobotModeData_V30()
(_, _,
rmd.timestamp, rmd.robot_connected, rmd.real_robot_enabled,
rmd.power_on_robot, rmd.emergency_stopped, rmd.security_stopped,
rmd.program_running, rmd.program_paused, rmd.robot_mode, rmd.control_mode,
rmd.target_speed_fraction, rmd.speed_scaling) = struct.unpack_from("!IBQ???????BBdd", buf)
return rmd
#this parses JointData for all versions (i.e. 1.6, 1.7, 1.8, 3.0)
class JointData(object):
__slots__ = ['q_actual', 'q_target', 'qd_actual',
'I_actual', 'V_actual', 'T_motor', 'T_micro', 'joint_mode']
......@@ -94,12 +126,13 @@ class JointData(object):
for i in range(6):
jd = JointData()
(jd.q_actual, jd.q_target, jd.qd_actual, jd.I_actual, jd.V_actual,
jd.T_motor, jd.T_micro,
jd.T_motor, jd.T_micro,
jd.joint_mode) = struct.unpack_from("!dddffffB", buf, offset)
offset += 41
all_joints.append(jd)
return all_joints
#this parses JointData for all versions (i.e. 1.6, 1.7, 1.8, 3.0)
class ToolData(object):
__slots__ = ['analog_input_range2', 'analog_input_range3',
'analog_input2', 'analog_input3',
......@@ -115,7 +148,23 @@ class ToolData(object):
td.tool_temperature, td.tool_mode) = struct.unpack_from("!IBbbddfBffB", buf)
return td
#this class handles different protocol versions
class MasterboardData(object):
@staticmethod
def unpack(buf):
md = MasterboardData()
(plen, ptype) = struct.unpack_from("!IB", buf)
if (plen == 64) or (plen == 76): # Euromap67 interface = 12 bytes
return MasterboardData_V18.unpack(buf)
elif (plen == 72) or (plen == 92): # Euromap67 interface = 20 bytes
return MasterboardData_V30.unpack(buf)
else:
print "MasterboardData has wrong length: " + str(plen)
print "Euromap67Interface is ignored"
return md
#this parses MasterboardData for versions <= v1.8 (i.e. 1.6, 1.7, 1.8)
class MasterboardData_V18(object):
__slots__ = ['digital_input_bits', 'digital_output_bits',
'analog_input_range0', 'analog_input_range1',
'analog_input0', 'analog_input1',
......@@ -124,10 +173,10 @@ class MasterboardData(object):
'masterboard_temperature',
'robot_voltage_48V', 'robot_current',
'master_io_current', 'master_safety_state',
'master_onoff_state']
'master_onoff_state']#subsequent slots related to 'euromap' ignored
@staticmethod
def unpack(buf):
md = MasterboardData()
md = MasterboardData_V18()
(_, _,
md.digital_input_bits, md.digital_output_bits,
md.analog_input_range0, md.analog_input_range1,
......@@ -140,6 +189,33 @@ class MasterboardData(object):
md.master_onoff_state) = struct.unpack_from("!IBhhbbddbbddffffBB", buf)
return md
#this parses MasterboardData for versions >=3.0 (i.e. 3.0)
class MasterboardData_V30(object):
__slots__ = ['digital_input_bits', 'digital_output_bits',
'analog_input_range0', 'analog_input_range1',
'analog_input0', 'analog_input1',
'analog_output_domain0', 'analog_output_domain1',
'analog_output0', 'analog_output1',
'masterboard_temperature',
'robot_voltage_48V', 'robot_current',
'master_io_current', 'safety_mode',
'in_reduced_mode']#subsequent slots related to 'euromap' ignored
@staticmethod
def unpack(buf):
md = MasterboardData_V30()
(_, _, _UNDOCUMENTED_,
md.digital_input_bits, md.digital_output_bits,
md.analog_input_range0, md.analog_input_range1,
md.analog_input0, md.analog_input1,
md.analog_output_domain0, md.analog_output_domain1,
md.analog_output0, md.analog_output1,
md.masterboard_temperature,
md.robot_voltage_48V, md.robot_current,
md.master_io_current, md.safety_mode,
md.in_reduced_mode) = struct.unpack_from("!IBIiibbddbbddffffBB", buf)
return md
#this parses JointData for all versions (i.e. 1.6, 1.7, 1.8, 3.0)
class CartesianInfo(object):
__slots__ = ['x', 'y', 'z', 'rx', 'ry', 'rz']
@staticmethod
......@@ -149,21 +225,26 @@ class CartesianInfo(object):
ci.x, ci.y, ci.z, ci.rx, ci.ry, ci.rz) = struct.unpack_from("!IB6d", buf)
return ci
#this parses KinematicsInfo for versions (i.e. 1.8, 3.0)
#KinematicsInfo is not available in 1.6 and 1.7
class KinematicsInfo(object):
@staticmethod
def unpack(buf):
return KinematicsInfo()
#helper class for ConfigurationData
class JointLimitData(object):
__slots__ = ['min_limit', 'max_limit', 'max_speed', 'max_acceleration']
#this parses ConfigurationData for versions (i.e. 1.8, 3.0)
#ConfigurationData is not available in 1.6 and 1.7
class ConfigurationData(object):
__slots__ = ['joint_limit_data',
'v_joint_default', 'a_joint_default',
'v_joint_default', 'a_joint_default',
'v_tool_default', 'a_tool_default', 'eq_radius',
'dh_a', 'dh_d', 'dh_alpha', 'dh_theta',
'masterboard_version', 'controller_box_type',
'robot_type', 'robot_subtype']
'robot_type', 'robot_subtype']#in v1.8 there is an additional slot 'motor_type' for each joint, which currently is ignored!
@staticmethod
def unpack(buf):
cd = ConfigurationData()
......@@ -179,6 +260,8 @@ class ConfigurationData(object):
cd.robot_subtype) = struct.unpack_from("!iiii", buf, 5+32*6+5*8+6*32)
return cd
#this parses KinematicsInfo for versions (i.e. 1.8, 3.0)
#KinematicsInfo is not available in 1.6 and 1.7
class ForceModeData(object):
__slots__ = ['x', 'y', 'z', 'rx', 'ry', 'rz', 'robot_dexterity']
@staticmethod
......@@ -188,6 +271,20 @@ class ForceModeData(object):
fmd.robot_dexterity) = struct.unpack_from("!IBddddddd", buf)
return fmd
#this class handles different protocol versions
class AdditionalInfo(object):
@staticmethod
def unpack(buf):
ai = AdditionalInfo()
(plen, ptype) = struct.unpack_from("!IB", buf)
if plen == 10:
return AdditionalInfoOld.unpack(buf)
elif plen == 7:
return AdditionalInfoNew.unpack(buf)
else:
print "AdditionalInfo has wrong length: " + str(plen)
return ai
class AdditionalInfoOld(object):
__slots__ = ['ctrl_bits', 'teach_button']
@staticmethod
......@@ -195,7 +292,7 @@ class AdditionalInfoOld(object):
ai = AdditionalInfoOld()
(_,_,ai.ctrl_bits, ai.teach_button) = struct.unpack_from("!IBIB", buf)
return ai
class AdditionalInfoNew(object):
__slots__ = ['teach_button_enabled','teach_button_pressed']
@staticmethod
......@@ -203,16 +300,7 @@ class AdditionalInfoNew(object):
ai = AdditionalInfoNew()
(_,_,ai.teach_button_enabled, ai.teach_button_pressed) = struct.unpack_from("!IBBB", buf)
return ai
class AdditionalInfo(object):
@staticmethod
def unpack(buf):
ai = AdditionalInfo()
(plen, ptype) = struct.unpack_from("!IB", buf)
if plen == 10:
return AdditionalInfoOld.unpack(buf)
else:
return AdditionalInfoNew.unpack(buf)
class RobotState(object):
__slots__ = ['robot_mode_data', 'joint_data', 'tool_data',
......
ur_driver/src/ur_driver/deserializeRT.py
+ 188
− 1
View file @ 6ee5c11f
import struct
import copy
#this class handles different protocol versions
class RobotStateRT(object):
@staticmethod
def unpack(buf):
rs = RobotStateRT()
(plen, ptype) = struct.unpack_from("!IB", buf)
if plen == 812:
return RobotStateRT_V18.unpack(buf)
elif plen == 1044:
return RobotStateRT_V30.unpack(buf)
else:
print "RobotStateRT has wrong length: " + str(plen)
return rs
#this parses RobotStateRT for versions <= v1.8 (i.e. 1.6, 1.7, 1.8)
class RobotStateRT_V18(object):
__slots__ = ['time',
'q_target', 'qd_target', 'qdd_target', 'i_target', 'm_target',
'q_actual', 'qd_actual', 'i_actual', 'tool_acc_values',
......@@ -20,7 +35,7 @@ class RobotStateRT(object):
print("MessageSize: ", message_size, "; BufferSize: ", len(buf))
raise Exception("Could not unpack RobotStateRT packet: length field is incorrect")
rs = RobotStateRT()
rs = RobotStateRT_V18()
#time: 1x double (1x 8byte)
rs.time = struct.unpack_from("!d",buf, offset)[0]
offset+=8
......@@ -116,3 +131,175 @@ class RobotStateRT(object):
rs.joint_modes = copy.deepcopy(all_values)
return rs
#this parses RobotStateRT for versions >=3.0 (i.e. 3.0)
class RobotStateRT_V30(object):
__slots__ = ['time',
'q_target', 'qd_target', 'qdd_target', 'i_target', 'm_target',
'q_actual', 'qd_actual', 'i_actual', 'i_control',
'tool_vector_actual', 'tcp_speed_actual', 'tcp_force',
'tool_vector_target', 'tcp_speed_target',
'digital_input_bits', 'motor_temperatures', 'controller_timer',
'test_value',
'robot_mode', 'joint_modes', 'safety_mode',
#6xd: unused
'tool_acc_values',
#6xd: unused
'speed_scaling', 'linear_momentum_norm',
#2xd: unused
'v_main', 'v_robot', 'i_robot', 'v_actual']
@staticmethod
def unpack(buf):
offset = 0
message_size = struct.unpack_from("!i", buf, offset)[0]
offset+=4
if message_size != len(buf):
print("MessageSize: ", message_size, "; BufferSize: ", len(buf))
raise Exception("Could not unpack RobotStateRT packet: length field is incorrect")
rs = RobotStateRT_V30()
#time: 1x double (1x 8byte)
rs.time = struct.unpack_from("!d",buf, offset)[0]
offset+=8
#q_target: 6x double (6x 8byte)
all_values = list(struct.unpack_from("!dddddd",buf, offset))
offset+=6*8
rs.q_target = copy.deepcopy(all_values)
#qd_target: 6x double (6x 8byte)
all_values = list(struct.unpack_from("!dddddd",buf, offset))
offset+=6*8
rs.qd_target = copy.deepcopy(all_values)
#qdd_target: 6x double (6x 8byte)
all_values = list(struct.unpack_from("!dddddd",buf, offset))
offset+=6*8
rs.qdd_target = copy.deepcopy(all_values)
#i_target: 6x double (6x 8byte)
all_values = list(struct.unpack_from("!dddddd",buf, offset))
offset+=6*8
rs.i_target = copy.deepcopy(all_values)
#m_target: 6x double (6x 8byte)
all_values = list(struct.unpack_from("!dddddd",buf, offset))
offset+=6*8
rs.m_target = copy.deepcopy(all_values)
#q_actual: 6x double (6x 8byte)
all_values = list(struct.unpack_from("!dddddd",buf, offset))
offset+=6*8
rs.q_actual = copy.deepcopy(all_values)
#qd_actual: 6x double (6x 8byte)
all_values = list(struct.unpack_from("!dddddd",buf, offset))
offset+=6*8
rs.qd_actual = copy.deepcopy(all_values)
#i_actual: 6x double (6x 8byte)
all_values = list(struct.unpack_from("!dddddd",buf, offset))
offset+=6*8
rs.i_actual = copy.deepcopy(all_values)
#i_control: 6x double (6x 8byte)
all_values = list(struct.unpack_from("!dddddd",buf, offset))
offset+=6*8
rs.i_control = copy.deepcopy(all_values)
#tool_vector_actual: 6x double (6x 8byte)
all_values = list(struct.unpack_from("!dddddd",buf, offset))
offset+=6*8
rs.tool_vector_actual = copy.deepcopy(all_values)
#tcp_speed_actual: 6x double (6x 8byte)
all_values = list(struct.unpack_from("!dddddd",buf, offset))
offset+=6*8
rs.tcp_speed_actual = copy.deepcopy(all_values)
#tcp_force: 6x double (6x 8byte)
all_values = list(struct.unpack_from("!dddddd",buf, offset))
offset+=6*8
rs.tcp_force = copy.deepcopy(all_values)
#tool_vector_target: 6x double (6x 8byte)
all_values = list(struct.unpack_from("!dddddd",buf, offset))
offset+=6*8
rs.tool_vector_target = copy.deepcopy(all_values)
#tcp_speed_target: 6x double (6x 8byte)
all_values = list(struct.unpack_from("!dddddd",buf, offset))
offset+=6*8
rs.tcp_speed_target = copy.deepcopy(all_values)
#digital_input_bits: 1x double (1x 8byte) ?
rs.digital_input_bits = struct.unpack_from("!d",buf, offset)[0]
offset+=8
#motor_temperatures: 6x double (6x 8byte)
all_values = list(struct.unpack_from("!dddddd",buf, offset))
offset+=6*8
rs.motor_temperatures = copy.deepcopy(all_values)
#controller_timer: 1x double (1x 8byte)
rs.controller_timer = struct.unpack_from("!d",buf, offset)[0]
offset+=8
#test_value: 1x double (1x 8byte)
rs.test_value = struct.unpack_from("!d",buf, offset)[0]
offset+=8
#robot_mode: 1x double (1x 8byte)
rs.robot_mode = struct.unpack_from("!d",buf, offset)[0]
offset+=8
#joint_modes: 6x double (6x 8byte)
all_values = list(struct.unpack_from("!dddddd",buf, offset))
offset+=6*8
rs.joint_modes = copy.deepcopy(all_values)
#safety_mode: 1x double (1x 8byte)
rs.safety_mode = struct.unpack_from("!d",buf, offset)[0]
offset+=8
#unused: 6x double (6x 8byte)
offset+=48
#tool_acc_values: 3x double (3x 8byte)
all_values = list(struct.unpack_from("!ddd",buf, offset))
offset+=3*8
rs.tool_acc_values = copy.deepcopy(all_values)
#unused: 6x double (6x 8byte)
offset+=48
#speed_scaling: 1x double (1x 8byte)
rs.speed_scaling = struct.unpack_from("!d",buf, offset)[0]
offset+=8
#linear_momentum_norm: 1x double (1x 8byte)
rs.linear_momentum_norm = struct.unpack_from("!d",buf, offset)[0]
offset+=8
#unused: 2x double (2x 8byte)
offset+=16
#v_main: 1x double (1x 8byte)
rs.v_main = struct.unpack_from("!d",buf, offset)[0]
offset+=8
#v_robot: 1x double (1x 8byte)
rs.v_robot = struct.unpack_from("!d",buf, offset)[0]
offset+=8
#i_robot: 1x double (1x 8byte)
rs.i_robot = struct.unpack_from("!d",buf, offset)[0]
offset+=8
#v_actual: 6x double (6x 8byte)
all_values = list(struct.unpack_from("!dddddd",buf, offset))
offset+=6*8
rs.v_actual = copy.deepcopy(all_values)
return rs
ur_driver/src/ur_driver/driver.py
+ 7
− 1
View file @ 6ee5c11f
......@@ -62,6 +62,10 @@ MULT_blend = 1000.0
MULT_analog = 1000000.0
MULT_analog_robotstate = 0.1
#Max Velocity accepted by ur_driver
MAX_VELOCITY = 10.0
#Using a very high value in order to not limit execution of trajectories being sent from MoveIt!
#Bounds for SetPayload service
MIN_PAYLOAD = 0.0
MAX_PAYLOAD = 1.0
......@@ -926,8 +930,10 @@ def main():
rospy.loginfo("No calibration offsets loaded from urdf")
# Reads the maximum velocity
# The max_velocity parameter is only used for debugging in the ur_driver. It's not related to actual velocity limits
global max_velocity
max_velocity = rospy.get_param("~max_velocity", 2.0)
max_velocity = rospy.get_param("~max_velocity", MAX_VELOCITY) # [rad/s]
rospy.loginfo("Max velocity accepted by ur_driver: %s [rad/s]" % max_velocity)
# Reads the minimum payload
global min_payload
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment