Skip to content

Commit

Permalink
Merge pull request #60 from UM-ARM-Lab/med
Browse files Browse the repository at this point in the history
Rework base robot classes and add Kuka iiwa Med support
  • Loading branch information
mvandermerwe authored May 17, 2021
2 parents 65825fd + cfefc2f commit 9e1fabc
Show file tree
Hide file tree
Showing 11 changed files with 402 additions and 126 deletions.
58 changes: 58 additions & 0 deletions arm_robots/launch/med.launch
Original file line number Diff line number Diff line change
@@ -0,0 +1,58 @@
<launch>
<arg name="sim" default="false"/>
<arg name="gui" default="true"/>
<arg name="update_description" default="true"/>
<arg name="verbose" default="false"/>

<group if="$(arg update_description)">
<!-- uploads robot description and setups up other parameters -->
<include file="$(find med_moveit_config)/launch/planning_context.launch">
<arg name="load_robot_description" value="true"/>
</include>
</group>


<group if="$(arg sim)">
<!-- Start gazebo -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="gui" value="$(arg gui)"/>
<arg name="paused" value="true"/>
<arg name="verbose" value="$(arg verbose)"/>
</include>

<!-- Spawn the robot in gazebo -->
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-urdf -model med -param robot_description"/>

<!-- start the ROS controllers, only used in gazebo. This merges gripper and arm joint states -->
<include file="$(find med_control)/launch/med_control_gazebo.launch">
<arg name="robot_namespace" value="med"/>
</include>

<!-- Move it-->
<include ns="med" file="$(find med_moveit_config)/launch/move_group.launch">
<arg name="allow_trajectory_execution" value="true"/>
<arg name="moveit_controller_manager" value="gazebo"/>
</include>
</group>
<group unless="$(arg sim)">
<!-- Start gazebo -->
<!-- <include file="$(find gazebo_ros)/launch/empty_world.launch"> -->
<!-- <arg name="gui" value="$(arg gui)"/> -->
<!-- <arg name="paused" value="false"/> -->
<!-- <arg name="verbose" value="$(arg verbose)"/> -->
<!-- <arg name="use_sim_time" value="false"/> -->
<!-- </include> -->

<!-- to let moveit send trajectories with the real robot -->
<include file="$(find arm_robots)/launch/trajectory_follower.launch">
<arg name="use_med" value="true"/>
</include>

<!-- MoveIt-->
<include ns="med" file="$(find med_moveit_config)/launch/move_group.launch">
<arg name="allow_trajectory_execution" value="true"/>
<arg name="moveit_controller_manager" value="real"/>
</include>
</group>
</launch>
11 changes: 11 additions & 0 deletions arm_robots/launch/trajectory_follower.launch
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,18 @@

<arg name="use_victor" default="false"/>
<arg name="use_val" default="false"/>
<arg name="use_med" default="false"/>

<group if="$(arg use_med)">
<node pkg="arm_robots" type="trajectory_follower_node.py" name="arms_trajectory_follower"
args="med arm_trajectory_controller" output="screen"/>
<group ns="med/static_transform_publishers">
<node pkg="tf2_ros" type="static_transform_publisher" name="active_robot_root_broadcaster" required="true"
args="0 0 0 0 0 0 med_base robot_root">
</node>
</group>
</group>

<group if="$(arg use_victor)">
<node pkg="arm_robots" type="trajectory_follower_node.py" name="arms_trajectory_follower"
args="victor both_arms_trajectory_controller"/>
Expand Down
76 changes: 28 additions & 48 deletions arm_robots/scripts/med_motion.py
100644 → 100755
Original file line number Diff line number Diff line change
Expand Up @@ -2,70 +2,50 @@
import rospy
from arm_robots.med import Med
import numpy as np
import pdb
import moveit_commander
from arc_utilities.conversions import convert_to_pose_msg
from victor_hardware_interface_msgs.msg import ControlMode


def main():
rospy.init_node('med_motion')

med = Med(manual_execute=True)
med.connect()
# med.set_grasping_force(40.0)
# med.open_gripper()
# Are we in sim? Currently WSG50 gripper not supported in Gazebo.
sim = rospy.get_param('~sim', default=True)

med.plan_to_joint_config(med.arm_group, [0,1.0,0,1.0,0,1.0,0])
med.plan_to_joint_config(med.arm_group, [0,0,0,0,0,0,0])
med = Med(display_goals=False)
med.connect()
med.set_control_mode(ControlMode.JOINT_POSITION, vel=0.1)

exit()
if not sim:
med.set_grasping_force(40.0)
med.open_gripper()

# Add table plane.
# scene = moveit_commander.PlanningSceneInterface(ns="victor")
# scene.add_plane('table_plane', convert_to_pose_msg([0,0,0,0,0,0]))
# Example: Planning to joint configuration.
med.plan_to_joint_config(med.arm_group, [0, 1.0, 0, -1.0, 0, 1.0, 0])

# Pick and place "demo"
med.plan_to_joint_config(med.arm_group, [0,0,0,0,0,0,0])
# Example: Planning to pose.
med.plan_to_pose(med.arm_group, med.wrist, [0.6, 0.12, 0.45, 0.0, np.pi, 0.0], frame_id='med_base')

while True:
_, result, _ = med.plan_to_pose(med.arm_group, med.wrist, [0.6, 0.12, 0.45, 0.0, np.pi, 0.0], frame_id='med_base')
if result.error_code < 0:
replan = input('Replan? [y/n]')
if replan != 'y':
exit()
else:
break
# Example: Cartesian planning.
med.plan_to_position_cartesian(med.arm_group, med.wrist, target_position=[0.6, 0.12, 0.35])

med.plan_to_joint_config(med.arm_group, [0,0,0,0,0,0,0])
exit()
# Example: Grasp object of approx. width 15mm
if not sim:
med.grasp(width=15.0)

med.plan_to_position_cartesian(med.arm_group, med.wrist, target_position=[0.6, -0.12, 0.35])
med.grasp(15.0)
med.plan_to_position_cartesian(med.arm_group, med.wrist, target_position=[0.6, -0.12, 0.45])
med.plan_to_position_cartesian(med.arm_group, med.wrist, target_position=[0.6, 0.12, 0.45])
med.plan_to_position_cartesian(med.arm_group, med.wrist, target_position=[0.6, 0.12, 0.365])
med.release()
med.plan_to_position_cartesian(med.arm_group, med.wrist, target_position=[0.6, 0.12, 0.45])
med.plan_to_joint_config(med.arm_group, [0,0,0,0,0,0,0])

# med.grasp(width=10.0)
# Plan to joint config.
# med.plan_to_joint_config(med.arm_group, [0,1.0,0,1.0,0,1.0,0])
# med.plan_to_joint_config(med.arm_group, [0,0,0,0,0,0,0])
med.plan_to_position_cartesian(med.arm_group, med.wrist, target_position=[0.6, -0.12, 0.45])
med.plan_to_position_cartesian(med.arm_group, med.wrist, target_position=[0.6, -0.12, 0.365])

# Plan to position (x,y,z) relative to robot base.
# med.plan_to_position(med.arm_group, med.wrist, [0.6, 0.0, 0.6])
# med.plan_to_joint_config(med.arm_group, [0,0,0,0,0,0,0])
# Example: Release grasp.
if not sim:
med.release()

# # Plan to pose relative to robot base.
# Example: Getting the robot state.
print("Current pose at the wrist:")
print(med.get_link_pose(med.wrist))

# med.release()
# med.plan_to_joint_config(med.arm_group, [0,0,0,0,0,0,0])
med.plan_to_joint_config(med.arm_group, [0, 0, 0, 0, 0, 0, 0])

# # Get and print current pose!
# print(med.get_link_pose(med.arm_group, med.wrist))
# med.plan_to_position_cartesian(med.arm_group, med.wrist, target_position=[0.6, 0.1, 0.35])
# med.plan_to_position_cartesian(med.arm_group, med.wrist, target_position=[0.6, -0.1, 0.35])
# med.plan_to_joint_config(med.arm_group, [0,0,0,0,0,0,0])

if __name__ == "__main__":
main()
33 changes: 2 additions & 31 deletions arm_robots/src/arm_robots/base_robot.py
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,8 @@
from trajectory_msgs.msg import JointTrajectoryPoint


class DualArmRobot:
class BaseRobot:

def __init__(self, robot_namespace: str = ''):
"""
This class is designed around the needs of the trajectory_follower.TrajectoryFollower
Expand All @@ -24,10 +25,6 @@ def __init__(self, robot_namespace: str = ''):
joint_states_topic = ns_join(self.robot_namespace, 'joint_states')
self._joint_state_listener = Listener(joint_states_topic, JointState)

# NOTE: derived classes must set these values
self.right_gripper_command_pub = None
self.left_gripper_command_pub = None

self.tf_wrapper = TF2Wrapper()

try:
Expand Down Expand Up @@ -78,29 +75,3 @@ def check_inputs(self, group_name: str, ee_link_name: str):
if group_name not in groups:
rospy.logwarn_throttle(1, f"Group [{group_name}] does not exist. Existing groups are:")
rospy.logwarn_throttle(1, groups)

def get_right_gripper_links(self):
return self.robot_commander.get_link_names("right_gripper")

def get_left_gripper_links(self):
return self.robot_commander.get_link_names("left_gripper")

def open_left_gripper(self):
self.left_gripper_command_pub.publish(self.get_open_gripper_msg())

def open_right_gripper(self):
self.right_gripper_command_pub.publish(self.get_open_gripper_msg())

def close_left_gripper(self):
# TODO: implementing blocking grasping
self.left_gripper_command_pub.publish(self.get_close_gripper_msg())

def close_right_gripper(self):
self.right_gripper_command_pub.publish(self.get_close_gripper_msg())

def get_close_gripper_msg(self):
# FIXME: this is a bad abstraction, not all grippers work like this
raise NotImplementedError()

def get_open_gripper_msg(self):
raise NotImplementedError()
19 changes: 19 additions & 0 deletions arm_robots/src/arm_robots/config/med_config.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,19 @@
import numpy as np

# Joint Names

ARM_JOINT_NAMES = (
"med_kuka_joint_1",
"med_kuka_joint_2",
"med_kuka_joint_3",
"med_kuka_joint_4",
"med_kuka_joint_5",
"med_kuka_joint_6",
"med_kuka_joint_7",
)

# From Med manual 4.3.2 and copied to kuka_iiwa_interface joint_limits.yaml
KUKA_MED_MAX_JOINT_VELOCITIES_DEG_S = (85.0, 85.0, 100.0, 75.0, 130.0, 135.0, 135.0)
KUKA_MED_MAX_JOINT_VELOCITIES = list(s * np.pi / 180 for s in KUKA_MED_MAX_JOINT_VELOCITIES_DEG_S)
KUKA_MED_MAX_JOINT_ACCEL_DEG_S2 = (50.0, 50.0, 100.0, 100.0, 1000.0, 650.0, 1500.0)
KUKA_MED_MAX_JOINT_ACCEL = list(s * np.pi / 180 for s in KUKA_MED_MAX_JOINT_ACCEL_DEG_S2)
9 changes: 6 additions & 3 deletions arm_robots/src/arm_robots/get_robot.py
Original file line number Diff line number Diff line change
Expand Up @@ -4,17 +4,18 @@
from arm_robots.hdt_michigan import Val, BaseVal
from arm_robots.robot import MoveitEnabledRobot
from arm_robots.victor import BaseVictor, Victor
from arm_robots.med import BaseMed, Med


def get_moveit_robot(robot_name: Optional[str] = None, **kwargs):
return get_robot(Val, Victor, robot_name, **kwargs)
return get_robot(Val, Victor, Med, robot_name, **kwargs)


def get_base_robot(robot_name: Optional[str] = None, **kwargs):
return get_robot(BaseVal, BaseVictor, robot_name, **kwargs)
return get_robot(BaseVal, BaseVictor, BaseMed, robot_name, **kwargs)


def get_robot(val_type: Type, victor_type: Type, robot_name: Optional[str] = None, **kwargs) -> MoveitEnabledRobot:
def get_robot(val_type: Type, victor_type: Type, med_type: Type, robot_name: Optional[str] = None, **kwargs) -> MoveitEnabledRobot:
"""
Get the right robot. It considers first the robot_name argument,
then checks the ros parameter server,
Expand All @@ -41,5 +42,7 @@ def get_robot(val_type: Type, victor_type: Type, robot_name: Optional[str] = Non
return victor_type(robot_name, **kwargs)
elif robot_name in ['val', 'hdt_michigan']:
return val_type('hdt_michigan', **kwargs)
elif robot_name == 'med':
return med_type(robot_name, **kwargs)
else:
raise NotImplementedError(f"robot with name {robot_name} not implemented")
24 changes: 21 additions & 3 deletions arm_robots/src/arm_robots/hdt_michigan.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,16 +9,16 @@
from colorama import Fore

import rospy
from arm_robots.base_robot import DualArmRobot
from arm_robots.base_robot import BaseRobot
from arm_robots.robot import MoveitEnabledRobot
from sensor_msgs.msg import JointState
from trajectory_msgs.msg import JointTrajectoryPoint


class BaseVal(DualArmRobot):
class BaseVal(BaseRobot):

def __init__(self, robot_namespace: str):
DualArmRobot.__init__(self, robot_namespace=robot_namespace)
BaseRobot.__init__(self, robot_namespace=robot_namespace)
self.first_valid_command = False
self.should_disconnect = False
self.min_velocity = 0.05
Expand Down Expand Up @@ -107,6 +107,11 @@ def send_joint_command(self, joint_names: List[str], trajectory_point: JointTraj
error_msg = ""
return failed, error_msg

def get_right_gripper_links(self):
return self.robot_commander.get_link_names("right_gripper")

def get_left_gripper_links(self):
return self.robot_commander.get_link_names("left_gripper")

left_arm_joints = [
'joint_1',
Expand Down Expand Up @@ -177,12 +182,19 @@ def close_left_gripper(self):
def close_right_gripper(self):
return self.set_right_gripper(self.gripper_closed_position)

def get_both_arm_joints(self):
return self.get_left_arm_joints() + self.get_right_arm_joints()

def get_right_arm_joints(self):
return right_arm_joints

def get_left_arm_joints(self):
return left_arm_joints

def get_gripper_positions(self):
# NOTE: this function requires that gazebo be playing
return self.get_link_pose(self.left_tool_name).position, self.get_link_pose(self.right_tool_name).position

def connect(self, **kwargs):
super().connect(**kwargs)
rospy.loginfo(Fore.GREEN + "Val ready!")
Expand All @@ -196,3 +208,9 @@ def is_gripper_closed(self, gripper: str):
raise NotImplementedError(f"invalid gripper {gripper}")
current_joint_values = move_group.get_current_joint_values()
return np.allclose(current_joint_values, self.gripper_closed_position, atol=0.01)

def is_left_gripper_closed(self):
return self.is_gripper_closed('left')

def is_right_gripper_closed(self):
return self.is_gripper_closed('right')
Loading

0 comments on commit 9e1fabc

Please sign in to comment.