class MoveitPlanner:
def __init__(self, manipulator_side = 'left'):
# Enable the robot at start and disable at the end
BaxterRobot().EnableBaxterRobot()
rospy.on_shutdown(BaxterRobot().DisableBaxterRobot)
# Test Open Both Grippers and Close Them
# baxter_manipulator = BaxterManipulatorAndGripper()
# baxter_manipulator.SetupBaxterManipualtor()
# baxter_manipulator.MoveBaxterManipulatorToNeutralPose()
# baxter_manipulator.SetupBaxterGrippers()
# baxter_manipulator.OpenGripper('left')
# baxter_manipulator.OpenGripper('right')
# rospy.sleep(3.0)
# baxter_manipulator.CloseGripper('left')
# baxter_manipulator.CloseGripper('right')
# Referenceing baxter arm
self.__manipulator_name = manipulator_side
self.limb = MoveGroupCommander("{}_arm".format(manipulator_side))
self.limb.set_end_effector_link("{}_gripper".format(manipulator_side))
# Baxter Model configuration with MoveIt not configured to control its gripper,
# thus using Baxter SDK API gripper class
baxter_manipulator = BaxterManipulatorAndGripper()
baxter_manipulator.SetupBaxterChosenGripperAndManipulator(manipulator_side)
self.limb.set_planner_id("RRTConnectKConfigDefualt")
self.limb.set_goal_position_tolerance(0.01)
self.limb.set_goal_orientation_tolerance(0.01)
class GenericDualArmClient():
_safe_kinematicsolver = "RRTConnectkConfigDefault"
def __init__(self, *args, **kwargs):
larm_name = args[0]
rarm_name = args[1] # "arm_right" for Motoman SDA10F
self.r_arm = MoveGroupCommander(rarm_name)
self.r_arm.set_planner_id(self._safe_kinematicsolver)
def move_rarm_shift_forward(self, axis, value):
'''
Ref. http://docs.ros.org/indigo/api/moveit_commander/html/classmoveit__commander_1_1move__group_1_1MoveGroupCommander.html#a22f9ec1477c3d61e728660850d50ce1f
'''
self.r_arm.shift_pose_target(axis, value)
self.r_arm.plan()
self.r_arm.go()
def move_rarm_fixedpose_forward(self):
tpose = Pose()
#TODO Currently the following position may work with SDA10F but not with NXO
tpose.position.x = 0.599
tpose.position.y = -0.379
tpose.position.z = 1.11
self.r_arm.set_pose_target(tpose)
self.r_arm.plan()
self.r_arm.go()
def callback(states):
scene = PlanningSceneInterface()
robot = RobotCommander()
arm = MoveGroupCommander("manipulator")
arm.set_planner_id("RRTConnectkConfigDefault")
arm.set_num_planning_attempts(20)
arm.allow_replanning(True)
pose = copy.deepcopy(states.pose[-1])
temp = tf.transformations.euler_from_quaternion(
(pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w))
quaternion = tf.transformations.quaternion_from_euler(
math.pi, temp[1], temp[2])
pose.position.z += 0.22
pose.orientation.x = quaternion[0]
pose.orientation.y = quaternion[1]
pose.orientation.z = quaternion[2]
pose.orientation.w = quaternion[3]
print pose
arm.set_pose_target(pose)
move_plan = arm.plan()
i = 0
while (not move_plan.joint_trajectory.joint_names):
move_plan = arm.plan()
i += 1
if (i == 5): break
state = arm.execute(move_plan)
class MoveItArm:
"""
Represents Baxter's Arm in MoveIt! world.
Represents a hand of Baxter through MoveIt! Group. Some reusable code has
been packed together into this class to make the code more readable.
"""
def __init__(self, side_name):
"""Will configure and initialise Baxter's hand."""
self._side_name = side_name
# This is the reference to Baxter's arm.
self.limb = MoveGroupCommander("{}_arm".format(side_name))
self.limb.set_end_effector_link("{}_gripper".format(side_name))
# Unfortunetly, MoveIt was not able to make the gripper work, neither
# did was able to find a very good pose using Forward Kinematics,
# so instead the Baxter SDK is used here to do these two jobs.
self.gripper = Gripper(side_name, CHECK_VERSION)
self.gripper.calibrate()
self._limb = Limb(side_name)
# This solver seems to be better for finding solution among obstacles
self.limb.set_planner_id("RRTConnectkConfigDefault")
# Error tollerance should be as low as possible for better accuracy.
self.limb.set_goal_position_tolerance(0.01)
self.limb.set_goal_orientation_tolerance(0.01)
def __str__(self):
"""
String representation of the arm.
String representation of the arm, either 'left' or 'right' string will
be returned. Useful when the string representation of the arm is
needed, like when accessing the IK solver.
"""
return self._side_name
def is_left(self):
"""Will return True if this is the left arm, false otherwise."""
return True if self._side_name == "left" else False
def is_right(self):
"""Will return True if this is the right arm, false otherwise."""
return True if self._side_name == "right" else False
def open_gripper(self):
"""Will open Baxter's gripper on his active hand."""
# Block ensures that the function does not return until the operation
# is completed.
self.gripper.open(block=True)
def close_gripper(self):
"""Will close Baxter's gripper on his active hand."""
# Block ensures that the function does not return until the operation
# is completed.
self.gripper.close(block=True)
def get_move_group_commander(group):
'''
Gets the move_group_commander for this process.
'''
global _mgc_dict
with _mgc_dict_creation_lock:
if not group in _mgc_dict:
_mgc_group = MoveGroupCommander(group)
_mgc_group.set_planner_id('RRTConnectkConfigDefault')
_mgc_dict[group] = _mgc_group
add_ground()
return _mgc_dict[group]
def get_move_group_commander(group):
'''
Gets the move_group_commander for this process.
'''
global _mgc_dict
with _mgc_dict_creation_lock:
if not group in _mgc_dict:
_mgc_group = MoveGroupCommander(group)
_mgc_group.set_planner_id('RRTConnectkConfigDefault')
_mgc_dict[group] = _mgc_group
add_ground()
return _mgc_dict[group]
def setUpClass(self):
rospy.init_node('test_moveit')
rospy.sleep(5) # intentinally wait for starting up hrpsys
self.robot = RobotCommander()
# TODO: Read groups from SRDF file ideally.
for mg_attr in self._MOVEGROUP_ATTRS:
mg = MoveGroupCommander(mg_attr[0])
# Temporary workaround of planner's issue similar to https://github.com/tork-a/rtmros_nextage/issues/170
mg.set_planner_id(self._KINEMATICSOLVER_SAFE)
# Append MoveGroup instance to the MoveGroup attribute list.
mg_attr.append(mg)
class MoveitWorkshopExercise(object):
def __init__(self):
rospy.init_node('commander_example', anonymous=True)
self.group_l = MoveGroupCommander("left_arm")
self.group_r = MoveGroupCommander("right_arm")
self.group_l.set_planner_id('RRTConnectkConfigDefault')
self.group_r.set_planner_id('RRTConnectkConfigDefault')
def exercise1(self):
pass
def exercise2(self):
pass
def exercise3(self):
pass
def main():
rospy.init_node('arm_start_up', anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
right_arm = MoveGroupCommander("right_arm")
left_arm = MoveGroupCommander("left_arm")
right_arm.set_planner_id("RRTConnect:");
left_arm.set_planner_id("RRTConnect:")
# move to a random target
#group.set_named_target("ahead")
#group.go()
#rospy.sleep(1)
right_arm.set_named_target("right_start")
right_arm.go()
rospy.sleep(1)
left_arm.set_named_target("left_start")
left_arm.go()
rospy.sleep(1)
class CalibrationMovements:
def __init__(self,
move_group_name,
max_velocity_scaling=0.5,
max_acceleration_scaling=0.5):
#self.client = HandeyeClient() # TODO: move around marker when eye_on_hand, automatically take samples via trigger topic
self.mgc = MoveGroupCommander(move_group_name)
self.mgc.set_planner_id("RRTConnectkConfigDefault")
self.mgc.set_max_velocity_scaling_factor(max_velocity_scaling)
self.mgc.set_max_acceleration_scaling_factor(max_acceleration_scaling)
self.start_pose = self.mgc.get_current_pose()
self.poses = []
self.current_pose_index = -1
self.fallback_joint_limits = [math.radians(90)] * 4 + [
math.radians(90)
] + [math.radians(180)] + [math.radians(350)]
if len(self.mgc.get_active_joints()) == 6:
self.fallback_joint_limits = self.fallback_joint_limits[1:]
def compute_poses_around_current_state(self, angle_delta,
translation_delta):
self.start_pose = self.mgc.get_current_pose()
basis = np.eye(3)
pos_deltas = [
quaternion_from_euler(*rot_axis * angle_delta)
for rot_axis in basis
]
neg_deltas = [
quaternion_from_euler(*rot_axis * (-angle_delta))
for rot_axis in basis
]
quaternion_deltas = list(
chain.from_iterable(izip(pos_deltas, neg_deltas))) # interleave
final_rots = []
for qd in quaternion_deltas:
final_rots.append(list(qd))
# TODO: clean up
pos_deltas = [
quaternion_from_euler(*rot_axis * angle_delta / 2)
for rot_axis in basis
]
neg_deltas = [
quaternion_from_euler(*rot_axis * (-angle_delta / 2))
for rot_axis in basis
]
quaternion_deltas = list(
chain.from_iterable(izip(pos_deltas, neg_deltas))) # interleave
for qd in quaternion_deltas:
final_rots.append(list(qd))
final_poses = []
for rot in final_rots:
fp = deepcopy(self.start_pose)
ori = fp.pose.orientation
combined_rot = quaternion_multiply([ori.x, ori.y, ori.z, ori.w],
rot)
fp.pose.orientation = Quaternion(*combined_rot)
final_poses.append(fp)
fp = deepcopy(self.start_pose)
fp.pose.position.x += translation_delta / 2
final_poses.append(fp)
fp = deepcopy(self.start_pose)
fp.pose.position.x -= translation_delta / 2
final_poses.append(fp)
fp = deepcopy(self.start_pose)
fp.pose.position.y += translation_delta
final_poses.append(fp)
fp = deepcopy(self.start_pose)
fp.pose.position.y -= translation_delta
final_poses.append(fp)
fp = deepcopy(self.start_pose)
fp.pose.position.z += translation_delta / 3
final_poses.append(fp)
self.poses = final_poses
self.current_pose_index = -1
def check_poses(self, joint_limits):
if len(self.fallback_joint_limits) == 6:
joint_limits = joint_limits[1:]
for fp in self.poses:
self.mgc.set_pose_target(fp)
plan = self.mgc.plan()
if len(plan.joint_trajectory.points
) == 0 or CalibrationMovements.is_crazy_plan(
plan, joint_limits):
return False
return True
def plan_to_start_pose(self):
return self.plan_to_pose(self.start_pose)
def plan_to_pose(self, pose):
self.mgc.set_start_state_to_current_state()
self.mgc.set_pose_target(pose)
plan = self.mgc.plan()
return plan
def execute_plan(self, plan):
if CalibrationMovements.is_crazy_plan(plan,
self.fallback_joint_limits):
raise RuntimeError("got crazy plan!")
self.mgc.execute(plan)
@staticmethod
def rot_per_joint(plan, degrees=False):
np_traj = np.array([p.positions for p in plan.joint_trajectory.points])
if len(np_traj) == 0:
raise ValueError
np_traj_max_per_joint = np_traj.max(axis=0)
np_traj_min_per_joint = np_traj.min(axis=0)
ret = abs(np_traj_max_per_joint - np_traj_min_per_joint)
if degrees:
ret = [math.degrees(j) for j in ret]
return ret
@staticmethod
def is_crazy_plan(plan, max_rotation_per_joint):
abs_rot_per_joint = CalibrationMovements.rot_per_joint(plan)
if (abs_rot_per_joint > max_rotation_per_joint).any():
return True
else:
return False
#!/usr/bin/env python
import rospy
import geometry_msgs.msg
from moveit_commander import MoveGroupCommander
if __name__ == '__main__':
rospy.init_node('commander_example', anonymous=True)
group = MoveGroupCommander("right_arm")
group.set_planner_id('RRTConnectkConfigDefault')
pose_rarm_curr = group.get_current_pose('RARM_JOINT5_Link')
rospy.loginfo('Current pose: {}'.format(pose_rarm_curr))
# right hand move to approx. 2cm ahead
pose_target = geometry_msgs.msg.Pose()
pose_target.position.x = 0.392
pose_target.position.y = -0.182
pose_target.position.z = 0.0748
pose_target.orientation.y = -0.708
pose_target.orientation.w = 0.706
rospy.loginfo("set target to {}".format(pose_target))
group.set_pose_target(pose_target)
plan = group.plan()
rospy.loginfo("plan is {}".format(plan))
ret = group.go()
rospy.loginfo("Command ran ... {}".format(ret))
pose_rarm_curr = group.get_current_pose('RARM_JOINT5_Link')
rospy.loginfo('Pose after: {}'.format(pose_rarm_curr))
class TestMoveit(unittest.TestCase):
_MOVEGROUP_MAIN = 'manipulator'
_KINEMATICSOLVER_SAFE = 'RRTConnectkConfigDefault'
@classmethod
def setUpClass(self):
rospy.init_node('test_moveit_vs060')
self.robot = RobotCommander()
self._mvgroup = MoveGroupCommander(self._MOVEGROUP_MAIN)
# Temporary workaround of planner's issue similar to https://github.com/tork-a/rtmros_nextage/issues/170
self._mvgroup.set_planner_id(self._KINEMATICSOLVER_SAFE)
self._movegroups = sorted(['manipulator', 'manipulator_flange'])
self._joints_movegroup_main = sorted(
['j1', 'j2', 'j3', 'j4', 'j5', 'flange'])
@classmethod
def tearDownClass(self):
True # TODO impl something meaningful
def _set_sample_pose(self):
'''
@return: Pose() with some values populated in.
'''
pose_target = Pose()
pose_target.orientation.x = 0.00
pose_target.orientation.y = 0.00
pose_target.orientation.z = -0.20
pose_target.orientation.w = 0.98
pose_target.position.x = 0.18
pose_target.position.y = 0.18
pose_target.position.z = 0.94
return pose_target
def _plan(self):
'''
Run `clear_pose_targets` at the beginning.
@rtype: RobotTrajectory http://docs.ros.org/api/moveit_msgs/html/msg/RobotTrajectory.html
'''
self._mvgroup.clear_pose_targets()
pose_target = self._set_sample_pose()
self._mvgroup.set_pose_target(pose_target)
plan = self._mvgroup.plan()
rospy.loginfo(' plan: '.format(plan))
return plan
def test_list_movegroups(self):
'''Check if the defined move groups are loaded.'''
self.assertEqual(self._movegroups,
sorted(self.robot.get_group_names()))
def test_list_activejoints(self):
'''Check if the defined joints in a move group are loaded.'''
self.assertEqual(self._joints_movegroup_main,
sorted(self._mvgroup.get_active_joints()))
def test_plan(self):
'''Evaluate plan (RobotTrajectory)'''
plan = self._plan()
# TODO Better way to check the plan is valid.
# Currently the following checks if the number of waypoints is not zero,
# which (hopefully) indicates that a path is computed.
self.assertNotEqual(0, plan.joint_trajectory.points)
def test_planandexecute(self):
'''
Evaluate Plan and Execute works.
Currently no value checking is done (, which is better to be implemented)
'''
self._plan()
# TODO Better way to check the plan is valid.
# The following checks if plan execution was successful or not.
self.assertTrue(self._mvgroup.go())
def test_set_pose_target(self):
'''
Check for simple planning, originally testd in moved from denso_vs060_moveit_demo_test.py
'''
self._plan()
p = [0.1, -0.35, 0.4]
pose = PoseStamped(
header=rospy.Header(stamp=rospy.Time.now(), frame_id='/BASE'),
pose=Pose(position=Point(*p),
orientation=Quaternion(
*quaternion_from_euler(1.57, 0, 1.57, 'sxyz'))))
self._mvgroup.set_pose_target(pose)
self.assertTrue(self._mvgroup.go() or self._mvgroup.go())
def test_planning_scene(self):
'''
Check if publish_simple_scene.py is working
'''
rospy.wait_for_service('/get_planning_scene', timeout=20)
get_planning_scene = rospy.ServiceProxy("/get_planning_scene",
GetPlanningScene)
collision_objects = []
# wait for 10 sec
time_start = rospy.Time.now()
while not collision_objects and (rospy.Time.now() -
time_start).to_sec() < 10.0:
world = get_planning_scene(
PlanningSceneComponents(components=PlanningSceneComponents.
WORLD_OBJECT_NAMES)).scene.world
collision_objects = world.collision_objects
rospy.loginfo("collision_objects = " +
str(world.collision_objects))
rospy.sleep(1)
self.assertTrue(world.collision_objects != [], world)
class RobotArm:
"""
A class used to represent the arm of a robot
using both "Moveit" API and Baxter SDK API.
"""
def __init__(self, side_name):
"""
Constructor.
@param side_name: which arm side we are refering to 'left' or 'right'.
"""
self.__side_name = side_name
# MoveIt interface to a group of arm joints.
# Either left arm joint group or right arm joint group.
self.moveit_limb = MoveGroupCommander('{}_arm'.format(side_name))
# MoveIt limb setting.
self.moveit_limb.set_end_effector_link('{}_gripper'.format(side_name))
self.moveit_limb.set_planner_id('RRTConnectKConfigDefault')
self.moveit_limb.set_goal_position_tolerance(0.01)
self.moveit_limb.set_goal_orientation_tolerance(0.01)
# MoveIt does not provide support for Baxter gripper.
# Thus we use Baxter SDK gripper instead.
self.gripper = Gripper(side_name, CHECK_VERSION)
self.gripper.calibrate()
self.baxter_sdk_limb = Limb(side_name)
def __str__(self):
"""
Built-in function to return a printable string
representing the arm used in either print(object)
or str(object).
"""
return str(self.__side_name)
def open_gripper(self):
"""
Command the robot arm end-effector i.e. the gripper to open.
"""
self.gripper.open(block=True)
def close_gripper(self):
"""
Command the robot arm end-effector i.e. the gripper to close.
"""
self.gripper.close(block=True)
def is_left_arm(self):
"""
Return true if the arm is corresponding to the left arm.
"""
return self.__side_name == 'left'
def is_right_arm(self):
"""
Return true if the arm is corresponding to the right arm.
"""
return self.__side_name == 'right'
class MoveIt(object):
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
self.scene = PlanningSceneInterface()
self.add_table()
# self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)
self.arm = MoveGroupCommander("arm")
# self.arm.set_goal_joint_tolerance(0.1)
self.gripper = MoveGroupCommander("gripper")
# already default
self.arm.set_planner_id("RRTConnectkConfigDefault")
self.end_effector_link = self.arm.get_end_effector_link()
self.arm.allow_replanning(True)
self.arm.set_planning_time(5)
self.transformer = tf.TransformListener()
rospy.sleep(2) # allow some time for initialization of moveit
def __del__(self):
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def _open_gripper(self):
joint_trajectory = JointTrajectory()
joint_trajectory.header.stamp = rospy.get_rostime()
joint_trajectory.joint_names = [
"m1n6s200_joint_finger_1", "m1n6s200_joint_finger_2"
]
joint_trajectory_point = JointTrajectoryPoint()
joint_trajectory_point.positions = [0, 0]
joint_trajectory_point.time_from_start = rospy.Duration(5.0)
joint_trajectory.points.append(joint_trajectory_point)
return joint_trajectory
def _set_gripper_width(self, width):
joint_trajectory = JointTrajectory()
joint_trajectory.header.stamp = rospy.get_rostime()
joint_trajectory.joint_names = [
"m1n6s200_joint_finger_1", "m1n6s200_joint_finger_2"
]
joint_trajectory_point = JointTrajectoryPoint()
joint_trajectory_point.positions = [0, 0]
joint_trajectory_point.time_from_start = rospy.Duration(5.0)
joint_trajectory.points.append(joint_trajectory_point)
return joint_trajectory
def _close_gripper(self):
joint_trajectory = JointTrajectory()
joint_trajectory.header.stamp = rospy.get_rostime()
joint_trajectory.joint_names = [
"m1n6s200_joint_finger_1", "m1n6s200_joint_finger_2"
]
joint_trajectory_point = JointTrajectoryPoint()
joint_trajectory_point.positions = [1.2, 1.2]
joint_trajectory_point.time_from_start = rospy.Duration(5.0)
joint_trajectory.points.append(joint_trajectory_point)
return joint_trajectory
# Template function for creating the Grasps
def _create_grasp(self, x, y, z, roll, pitch, yaw):
grasp = Grasp()
# pre_grasp
grasp.pre_grasp_posture = self._open_gripper()
grasp.pre_grasp_approach.direction.header.frame_id = self.end_effector_link
grasp.pre_grasp_approach.direction.vector.z = 1.0
grasp.pre_grasp_approach.direction.vector.y = 0.0
grasp.pre_grasp_approach.direction.vector.x = 0.0
grasp.pre_grasp_approach.min_distance = 0.05
grasp.pre_grasp_approach.desired_distance = 0.1
# grasp
grasp.grasp_posture = self._close_gripper()
grasp.grasp_pose.pose.position.x = x
grasp.grasp_pose.pose.position.y = y
grasp.grasp_pose.pose.position.z = z
q = quaternion_from_euler(roll, pitch, yaw)
grasp.grasp_pose.pose.orientation.x = q[0]
grasp.grasp_pose.pose.orientation.y = q[1]
grasp.grasp_pose.pose.orientation.z = q[2]
grasp.grasp_pose.pose.orientation.w = q[3]
grasp.grasp_pose.header.frame_id = "m1n6s200_link_base"
# post_grasp
grasp.post_grasp_retreat.direction.header.frame_id = self.end_effector_link
grasp.post_grasp_retreat.direction.vector.z = -1.0
grasp.post_grasp_retreat.direction.vector.x = 0.0
grasp.post_grasp_retreat.direction.vector.y = 0.0
grasp.post_grasp_retreat.min_distance = 0.05
grasp.post_grasp_retreat.desired_distance = 0.25
return [grasp]
# Template function, you can add parameters if needed!
def grasp(self, x, y, z, roll, pitch, yaw, width):
self.add_object('object', [0.37, -0.24, 0.1, math.pi, 0., 0.],
[0.1, 0.1, 0.1])
grasps = self._create_grasp(x, y, z, roll, pitch, yaw)
result = self.arm.pick('object', grasps)
self.remove_object()
if result == MoveItErrorCodes.SUCCESS:
print 'Success grasp'
return True
else:
print 'Failed grasp'
return False
def open_fingers(self):
self.gripper.set_joint_value_target([0.0, 0.0])
self.gripper.go(wait=True)
rospy.sleep(2.0)
def close_fingers(self):
self.gripper.set_joint_value_target([1.3, 1.3])
self.gripper.go(wait=True)
rospy.sleep(2.0)
def move_to(self,
x,
y,
z,
roll,
pitch,
yaw,
frame_id="m1n6s200_link_base"):
q = quaternion_from_euler(roll, pitch, yaw)
pose = PoseStamped()
pose.header.frame_id = frame_id
pose.pose.position.x = x
pose.pose.position.y = y
pose.pose.position.z = z
pose.pose.orientation.x = q[0]
pose.pose.orientation.y = q[1]
pose.pose.orientation.z = q[2]
pose.pose.orientation.w = q[3]
self.arm.set_pose_target(pose, self.end_effector_link)
plan = self.arm.plan()
success = True
success = self.arm.go(wait=True)
self.arm.stop()
self.arm.clear_pose_targets()
return success
def remove_object(self, object='object'):
self.scene.remove_attached_object(self.end_effector_link, object)
self.scene.remove_world_object(object)
rospy.loginfo("Object removed")
def add_object(self, name, pose, size):
object_pose = PoseStamped()
object_pose.header.frame_id = "m1n6s200_link_base"
object_pose.pose.position.x = pose[0]
object_pose.pose.position.y = pose[1]
object_pose.pose.position.z = pose[2]
q = quaternion_from_euler(*pose[3:])
object_pose.pose.orientation.x = q[0]
object_pose.pose.orientation.y = q[1]
object_pose.pose.orientation.z = q[2]
object_pose.pose.orientation.w = q[3]
self.scene.add_box(name, object_pose, size)
def add_table(self):
self.add_object('table', [0, 0, -0.005, 0, 0, 0], (2, 2, 0.01))
class MoveIt(object):
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
self.scene = PlanningSceneInterface()
self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)
self.arm = MoveGroupCommander("arm")
# self.arm.set_goal_joint_tolerance(0.1)
self.gripper = MoveGroupCommander("gripper")
# already default
self.arm.set_planner_id("RRTConnectkConfigDefault")
self.end_effector_link = self.arm.get_end_effector_link()
self.arm.allow_replanning(True)
self.arm.set_planning_time(5)
self.transformer = tf.TransformListener()
rospy.sleep(2) # allow some time for initialization of moveit
def __del__(self):
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def _open_gripper(self):
joint_trajectory = JointTrajectory()
joint_trajectory.header.stamp = rospy.get_rostime()
joint_trajectory.joint_names = [
"m1n6s200_joint_finger_1", "m1n6s200_joint_finger_2"
]
joint_trajectory_point = JointTrajectoryPoint()
joint_trajectory_point.positions = [0, 0]
joint_trajectory_point.time_from_start = rospy.Duration(5.0)
joint_trajectory.points.append(joint_trajectory_point)
return joint_trajectory
def _close_gripper(self):
joint_trajectory = JointTrajectory()
joint_trajectory.header.stamp = rospy.get_rostime()
joint_trajectory.joint_names = [
"m1n6s200_joint_finger_1", "m1n6s200_joint_finger_2"
]
joint_trajectory_point = JointTrajectoryPoint()
joint_trajectory_point.positions = [1.2, 1.2]
joint_trajectory_point.time_from_start = rospy.Duration(5.0)
joint_trajectory.points.append(joint_trajectory_point)
return joint_trajectory
# Template function for creating the Grasps
def _create_grasps(self, x, y, z, z_max, rotation):
grasps = []
# You can create multiple grasps and add them to the grasps list
grasp = Grasp() # create a new grasp
# Set the pre grasp posture (the fingers)
grasp.pre_grasp_posture = self._open_gripper()
# Set the grasp posture (the fingers)
grasp.grasp_posture = self._close_gripper()
# Set the position of where to grasp
grasp.grasp_pose.pose.position.x = x
grasp.grasp_pose.pose.position.y = y
grasp.grasp_pose.pose.position.z = z
# Set the orientation of the end effector
q = quaternion_from_euler(math.pi, 0.0, rotation)
grasp.grasp_pose.pose.orientation.x = q[0]
grasp.grasp_pose.pose.orientation.y = q[1]
grasp.grasp_pose.pose.orientation.z = q[2]
grasp.grasp_pose.pose.orientation.w = q[3]
grasp.grasp_pose.header.frame_id = "m1n6s200_link_base"
# Set the pre_grasp_approach
grasp.pre_grasp_approach.direction.header.frame_id = self.end_effector_link
grasp.pre_grasp_approach.direction.vector.z = 1.0
grasp.pre_grasp_approach.direction.vector.y = 0.0
grasp.pre_grasp_approach.direction.vector.x = 0.0
grasp.pre_grasp_approach.min_distance = 0.05
grasp.pre_grasp_approach.desired_distance = 0.1
# # Set the post_grasp_approach
grasp.post_grasp_retreat.direction.header.frame_id = self.end_effector_link
grasp.post_grasp_retreat.direction.vector.z = -1.0
grasp.post_grasp_retreat.direction.vector.x = 0.0
grasp.post_grasp_retreat.direction.vector.y = 0.0
grasp.post_grasp_retreat.min_distance = 0.05
grasp.post_grasp_retreat.desired_distance = 0.25
grasp.grasp_pose.header.frame_id = "m1n6s200_link_base" # setting the planning frame (Positive x is to the left, negative Y is to the front of the arm)
grasps.append(
grasp
) # add all your grasps in the grasps list, MoveIT will pick the best one
for z_offset in np.arange(z + 0.02, z_max, 0.01):
new_grasp = copy.deepcopy(grasp)
new_grasp.grasp_pose.pose.position.z = z_offset
grasps.append(new_grasp)
return grasps
# Template function, you can add parameters if needed!
def grasp(self, x, y, z, z_max, rotation, size):
print '******************* grasp'
# Object distance:
obj_dist = np.linalg.norm(np.asarray((x, y, z)))
if obj_dist > 0.5:
rospy.loginfo(
"Object too far appart ({} m), skipping pick".format(obj_dist))
return False
# Add collision object, easiest to name the object, "object"
object_pose = PoseStamped()
object_pose.header.frame_id = "m1n6s200_link_base"
object_pose.pose.position.x = x
object_pose.pose.position.y = y
object_pose.pose.position.z = z
q = quaternion_from_euler(math.pi, 0.0, rotation)
object_pose.pose.orientation.x = q[0]
object_pose.pose.orientation.y = q[1]
object_pose.pose.orientation.z = q[2]
object_pose.pose.orientation.w = q[3]
self.scene.add_box("object", object_pose, size)
rospy.sleep(0.5)
self.clear_octomap()
rospy.sleep(1.0)
# Create and return grasps
# z += size[2]/2 # Focus on the top of the object only
# z += size[2]/2 + 0.02 # Focus on the top of the object only
grasps = self._create_grasps(x, y, z, z_max, rotation)
print '******************************************************************************'
result = self.arm.pick(
'object', grasps) # Perform pick on "object", returns result
print '******************************************************************************'
# self.move_to(x, y, z + 0.15, rotation)
if result == MoveItErrorCodes.SUCCESS:
print 'Success grasp'
return True
else:
print 'Failed grasp'
return False
def clear_object(self, x, y, z, z_max, rotation, size):
print '******************* clear_object'
self.move_to_waypoint()
success = self.grasp(x, y, z, z_max, rotation, size)
if success:
self.move_to_waypoint()
success = self.move_to_drop_zone()
if success:
print 'success move to drop zone'
else:
print 'failed move to drop zone'
self.open_fingers()
self.remove_object()
rospy.sleep(1.0)
self.close_fingers()
return success
def open_fingers(self):
print '******************* open_fingers'
self.gripper.set_joint_value_target([0.0, 0.0])
self.gripper.go(wait=True)
rospy.sleep(2.0)
def close_fingers(self):
print '******************* close_fingers'
self.gripper.set_joint_value_target([1.3, 1.3])
self.gripper.go(wait=True)
rospy.sleep(2.0)
def move_to(self, x, y, z, rotation, frame_id="m1n6s200_link_base"):
print '******************* move_to'
q = quaternion_from_euler(math.pi, 0.0, rotation)
pose = PoseStamped()
pose.header.frame_id = frame_id
pose.pose.position.x = x
pose.pose.position.y = y
pose.pose.position.z = z
pose.pose.orientation.x = q[0]
pose.pose.orientation.y = q[1]
pose.pose.orientation.z = q[2]
pose.pose.orientation.w = q[3]
self.arm.set_pose_target(pose, self.end_effector_link)
plan = self.arm.plan()
success = self.arm.go(wait=True)
self.arm.stop()
self.arm.clear_pose_targets()
return success
def move_to_waypoint(self):
print '******************* move_to_waypoint'
return self.move_to(0.35, 0, 0.25, 1.57)
def rtb(self):
print '******************* rtb'
self.move_to_drop_zone()
# pose = PoseStamped()
# pose.header.frame_id = 'base_footprint'
# pose.pose.position.x = -0.191258927921
# pose.pose.position.y = 0.1849306168113
# pose.pose.position.z = 0.813729734732
# pose.pose.orientation.x = -0.934842026356
# pose.pose.orientation.y = 0.350652799078
# pose.pose.orientation.z = -0.00168532388516
# pose.pose.orientation.w = 0.0557688079539
#
# self.arm.set_pose_target(pose, self.end_effector_link)
# plan = self.arm.plan()
# self.arm.go(wait=True)
# self.arm.stop()
# self.arm.clear_pose_targets()
def move_to_drop_zone(self):
print '******************* move_to_drop_zone'
pose = PoseStamped()
pose.header.frame_id = "m1n6s200_link_base"
pose.pose.position.x = 0.2175546259709541
pose.pose.position.y = 0.18347985269448372
pose.pose.position.z = 0.16757751444136426
pose.pose.orientation.x = 0.6934210704552356
pose.pose.orientation.y = 0.6589390059796749
pose.pose.orientation.z = -0.23223137602833943
pose.pose.orientation.w = -0.17616808290725341
self.arm.set_pose_target(pose, self.end_effector_link)
plan = self.arm.plan()
success = self.arm.go(wait=True)
self.arm.stop()
self.arm.clear_pose_targets()
return success
def print_position(self):
pose = self.arm.get_current_pose()
self.transformer.waitForTransform("m1n6s200_link_base",
"base_footprint", rospy.Time.now(),
rospy.Duration(10))
eef_pose = self.transformer.transformPose("m1n6s200_link_base", pose)
orientation = eef_pose.pose.orientation
orientation = [
orientation.x, orientation.y, orientation.z, orientation.w
]
euler = euler_from_quaternion(orientation)
print "z:", eef_pose.pose.position.x
print "y:", eef_pose.pose.position.y
print "z:", eef_pose.pose.position.z
print "yaw (degrees):", math.degrees(euler[2])
def remove_object(self):
self.scene.remove_attached_object(self.end_effector_link, "object")
self.scene.remove_world_object("object")
rospy.loginfo("Object removed")
from moveit_commander import RobotCommander, MoveGroupCommander
from moveit_commander import PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
from geometry_msgs.msg import PoseStamped
from moveit_msgs.msg import Grasp, GripperTranslation, PlaceLocation
from trajectory_msgs.msg import JointTrajectoryPoint
if __name__=='__main__':
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
GRIPPER_FRAME = 'right_gripper_link'
scene = PlanningSceneInterface()
robot = RobotCommander()
right_arm = MoveGroupCommander("right_arm")
right_gripper = MoveGroupCommander("right_gripper")
right_arm.set_planner_id("KPIECEkConfigDefault");
rospy.sleep(1)
# clean the scene
scene.remove_world_object("table")
scene.remove_world_object("part")
scene.remove_attached_object(GRIPPER_FRAME, "part")
#rospy.logwarn("cleaning world")
#right_arm.set_named_target("r_start")
#right_arm.go()
#right_gripper.set_named_target("open")
#right_gripper.go()
rospy.sleep(3)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Construct the initial scene object
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# Create a dictionary to hold object colors
self.colors = dict()
# Pause for the scene to get ready
rospy.sleep(1)
# Initialize the move group for the right arm
right_arm = MoveGroupCommander('right_arm')
# Initialize the move group for the left arm
left_arm = MoveGroupCommander('left_arm')
right_arm.set_planner_id("KPIECEkConfigDefault")
left_arm.set_planner_id("KPIECEkConfigDefault")
rospy.sleep(1)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.01)
right_arm.set_goal_orientation_tolerance(0.05)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the reference frame for pose targets
reference_frame = 'base_footprint'
# Set the right arm reference frame accordingly
right_arm.set_pose_reference_frame(reference_frame)
# Allow 5 seconds per planning attempt
right_arm.set_planning_time(5)
# Give each of the scene objects a unique name
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
#left_arm.set_named_target('left_start')
#left_arm.go()
# Start the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_start')
right_arm.go()
rospy.sleep(2)
# Set the height of the table off the ground
table_ground = 0.75
# Set the length, width and height of the table and boxes
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# Add a table top and two boxes to the scene
table_pose = PoseStamped()
table_pose.header.frame_id = reference_frame
table_pose.pose.position.x = 0.56
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = reference_frame
box1_pose.pose.position.x = 0.51
box1_pose.pose.position.y = -0.1
box1_pose.pose.position.z = table_ground + table_size[
2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = reference_frame
box2_pose.pose.position.x = 0.49
box2_pose.pose.position.y = 0.15
box2_pose.pose.position.z = table_ground + table_size[
2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
# Make the table red and the boxes orange
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the target pose in between the boxes and above the table
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.pose.position.x = 0.4
target_pose.pose.position.y = 0.0
target_pose.pose.position.z = table_pose.pose.position.z + table_size[
2] + 0.05
target_pose.pose.orientation.w = 1.0
# Set the target pose for the arm
right_arm.set_pose_target(target_pose, end_effector_link)
# Move the arm to the target pose (if possible)
right_arm.go()
# Pause for a moment...
rospy.sleep(2)
# Return the arm to the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_start')
right_arm.go()
# Exit MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
class UR5_Gripped_Manipulator:
"""
Class Attributes
Robot : Robot Commander Object
Scene : Planning Scene Interface Object (current scene)
Man : MoveGroupCommander object to control manipulator
EEF : MoveGroupCommander object to control endeffector
Target_poses : Pose list of created target objects
Picked : List of part indexes to ensure to picked non-picked ones
"""
def __init__(self, manip_name="manipulator", eef_name="endeffector"):
self.robot = RobotCommander()
self.man = MoveGroupCommander(manip_name)
self.eef = MoveGroupCommander(eef_name)
self.target_poses = []
self.picked = []
"""
----------------Function Name: clean_scene----------------
Definition: Clears the target_poses and picked lists in order to establish a new execution. Necessary for test_simulation method
"""
def clear_poses(self):
for i in xrange(len(self.target_poses)):
self.target_poses.pop()
for i in xrange(len(self.picked)):
self.picked.pop()
"""
----------------Function Name: clean_scene----------------
Definition : Clears all non-attached objects from planning scene
Group : MoveGroupCommander object to control given group
"""
def default_state_gripper(self, grp):
rospy.loginfo("Openning Gripper")
joint_vals = grp.get_current_joint_values()
joint_vals[0] = 0.0
grp.set_joint_value_target(joint_vals)
init_plan = grp.plan()
return grp.execute(init_plan)
"""
----------------Function Name: closed_state_gripper----------------
Definition : Function that opens gripper and detachs the gripped object
Obj : Name of the Object that is needed to detach
"""
def closed_state_gripper(self, curr_scene, obj):
rospy.loginfo("Closing Gripper")
def convert(width):
return 0.77 - width / 0.15316 * 0.77
width = curr_scene.get_objects([obj])[obj].primitives[0].dimensions[0]
width = convert(width)
now = self.robot.endeffector.get_current_joint_values()[0]
cnt = 0
while abs(now - width) > 0.05:
now = self.robot.endeffector.get_current_joint_values()[0]
cnt = cnt + 1
tmp = width - abs(now - width) / 2.0
self.robot.endeffector.set_joint_value_target('finger_joint', tmp)
self.robot.endeffector.go()
rospy.sleep(0.05)
if cnt == 7:
break
rospy.sleep(1.0)
ret = self.robot.manipulator.attach_object(obj)
return ret
"""
----------------Function Name: set_mid_state----------------
Definition : Motion planning function for defined mid state for manipulator, can be used for enhancing the performance if needed
Group : MoveGroupCommander object to control given group
"""
def set_mid_state(self, group):
rospy.loginfo("Going to Mid-State")
pos = PoseStamped()
pos.header.frame_id = self.robot.get_planning_frame()
pos.pose.position.x = 0.45358
pos.pose.position.y = 0.188518
pos.pose.position.z = 0.473063
pos.pose.orientation.x = 0.0004738
pos.pose.orientation.y = 1
pos.pose.orientation.z = 0
pos.pose.orientation.w = 0.000155894
group.set_pose_target(pos)
move_plan = group.plan()
while (not move_plan.joint_trajectory.joint_names):
move_plan = group.plan()
return group.execute(move_plan)
"""
----------------Function Name: add_object----------------
Name : Object Name
Pose : Pose of the Object (x,y,z,xo,yo,zo,wo)
Dimension : Dimensions of the Obhect (Tuple) (d1,d2,d3),
Type : Box(0),Sphere(1)
d1 is radius for sphere i.e typ==1,
"""
def add_object(self,
name,
x,
y,
z,
xo=0.0,
yo=0.0,
zo=0.0,
wo=0.0,
d1=0.1,
d2=0.1,
d3=0.1,
curr_scene=None,
typ=0):
pos = PoseStamped()
pos.header.frame_id = self.robot.get_planning_frame()
pos.pose.position.x = x
pos.pose.position.y = y
pos.pose.position.z = z
pos.pose.orientation.x = xo
pos.pose.orientation.y = yo
pos.pose.orientation.z = zo
pos.pose.orientation.w = wo
rospy.loginfo("ADDING OBJECT")
if (typ == 0):
curr_scene.add_box(name, pos, (d1, d2, d3))
elif (typ == 1):
curr_scene.add_sphere(name, pos, d1)
else:
print("ERROR in Type")
return pos
"""
----------------Function Name: clean_scene----------------
Definition: Clears all non-attached objects from planning scene
"""
def clean_scene(self, curr_scene):
rospy.loginfo("Clearing the Scene")
curr_scene.remove_world_object()
"""
----------------Function Name: create_environment----------------
Definition : Creates the simulation environment with non-target collision objects, can be edited to create different default environment
"""
def create_environment(self, curr_scene):
self.add_object(name="wall",
x=0.0,
y=0.8,
z=0.5,
d1=0.1,
d2=0.35,
d3=1,
curr_scene=curr_scene,
typ=0)
self.add_object(name="wall_2",
x=0.0,
y=-0.8,
z=0.5,
d1=0.1,
d2=0.35,
d3=1,
curr_scene=curr_scene,
typ=0)
self.add_object(name="table",
x=0.0,
y=0.0,
z=-0.05,
d1=2,
d2=2,
d3=0.0001,
curr_scene=curr_scene,
typ=0)
"""
----------------Function Name: check_targets----------------
Definition : Creates given number of objects within the distance provided. It prevents collision object overlapping.
It returns the pose list of the objects
Number : Number of collision objects required to spawn
Distance : Required minimum distance between each collision objects
"""
def check_targets(self, curr_scene, number, distance):
rospy.loginfo("Creating Boxes!")
def al(typ, x=0.0):
if typ == 'x':
return random.uniform(0.35, 0.65)
elif typ == "y":
rang = math.sqrt(0.7**2 - x**2)
return random.uniform(-rang, rang)
hemme = []
dims = []
cnt = 0
while len(hemme) != number:
if cnt == 200:
hemme = []
cnt = 0
cnt = cnt + 1
dim = (random.uniform(0.08, 0.12), random.uniform(0.08, 0.10),
random.uniform(0.05, 0.2))
quaternion = tf.transformations.quaternion_from_euler(
0.0, 0.0, random.uniform(-math.pi, math.pi))
box = PoseStamped()
box.header.frame_id = self.robot.get_planning_frame()
box.pose.position.z = (-0.04 + (dim[2] / 2))
box.pose.position.x = al('x')
box.pose.position.y = al('y', box.pose.position.x)
box.pose.orientation.x = quaternion[0]
box.pose.orientation.y = quaternion[1]
box.pose.orientation.z = quaternion[2]
box.pose.orientation.w = quaternion[3]
flag = 1
for i in hemme:
if abs(i.pose.position.x -
box.pose.position.x) < distance or abs(
i.pose.position.y - box.pose.position.y) < distance:
flag = 0
if flag == 0:
continue
hemme.append(box)
dims.append(dim)
cnt = 0
names = []
for i in xrange(len(hemme)):
now = "part" + str(cnt)
cnt = cnt + 1
names.append(now)
self.add_object(name=now,
x=hemme[i].pose.position.x,
y=hemme[i].pose.position.y,
z=hemme[i].pose.position.z,
xo=hemme[i].pose.orientation.x,
yo=hemme[i].pose.orientation.y,
zo=hemme[i].pose.orientation.z,
wo=hemme[i].pose.orientation.w,
d1=dims[i][0],
d2=dims[i][1],
d3=dims[i][2],
curr_scene=curr_scene,
typ=0)
print("End Check!")
return hemme
"""
----------------Function Name: pick_object----------------
Definition : It moves the given group i.e robot to the collision object whose index is given and picks that object.
Group : MoveGroupCommander object to control given group
Part_index : Index of the target object to obtain pose
"""
def pick_object(self, curr_scene, group, part_index):
rospy.loginfo("Pick Operation starts!")
gripped_object = curr_scene.get_objects(["part" + str(part_index)
])["part" + str(part_index)]
pos = copy.deepcopy(self.target_poses[part_index])
temp = tf.transformations.euler_from_quaternion(
(pos.pose.orientation.x, pos.pose.orientation.y,
pos.pose.orientation.z, pos.pose.orientation.w))
quaternion = tf.transformations.quaternion_from_euler(
math.pi, temp[1], temp[2])
pos.pose.position.z += 0.17 + (
gripped_object.primitives[0].dimensions[2] / 2.0)
pos.pose.orientation.y = 1
group.set_pose_target(pos)
move_plan = group.plan()
i = 0
while (not move_plan.joint_trajectory.joint_names):
move_plan = group.plan()
i += 1
if (i == 500): break
state = group.execute(move_plan)
rospy.loginfo("Execute operation for Object is %s" % str(part_index))
if (state):
self.closed_state_gripper(curr_scene, "part" + str(part_index))
rospy.sleep(1.0)
self.place_object(curr_scene, group, part_index)
return
else:
return
"""
----------------Function Name: place_object----------------
Definition : It places the gripped object to the target location
Group : MoveGroupCommander object to control given group
Part_index : Index of the target object to obtain pose
"""
def place_object(self, curr_scene, group, part_index):
pos = PoseStamped()
def al(typ, x=0.0):
if typ == 'x':
return random.uniform(-0.35, -0.6)
elif typ == 'y':
rang = math.sqrt(0.75**2 - x**2)
x = random.uniform(-rang, rang)
while abs(x) < 0.15:
x = random.uniform(-rang, rang)
return x
pos = PoseStamped()
pos.header.frame_id = robot.get_planning_frame()
pos.pose.position.x = al("x")
pos.pose.position.y = al("y", pos.pose.position.x)
#This line makes placing possible by setting a valid z position for gripped object
pos.pose.position.z = -0.04 + 0.17 + ((curr_scene.get_attached_objects(
["part" + str(part_index)
])["part" + str(part_index)]).object.primitives[0].dimensions[2])
pos.pose.orientation.y = 1.0
group.set_pose_target(pos)
move_plan = group.plan()
while (not move_plan.joint_trajectory.joint_names):
move_plan = group.plan()
state = group.execute(move_plan)
if (state):
detached = group.detach_object("part" + str(part_index))
rospy.sleep(1.5)
if (detached):
curr_scene.remove_world_object("part" + str(part_index))
rospy.sleep(1.5)
self.default_state_gripper(self.eef)
self.picked.append(part_index)
else:
self.default_state_gripper(robot.endeffector)
group.detach_object("part" + str(part_index))
rospy.sleep(2)
"""
----------------Function Name: execute_simulation----------------
Definition : Creates the specified environment , generates motion plans and does the pick and place operation based on these plans. To visualize RViz is needed.
Planner ID can be changed based on MoveIT's supported OMPL.
"""
def execute_simulation(self, curr_scene=None, num_of_objects=3):
is_success = False
#Reset State of the Gripper
self.default_state_gripper(self.eef)
####
#Reset the position of the Arm
# Will be implemented if needed
####
#Clean the scene
self.clean_scene(curr_scene=curr_scene)
#Create environment
self.create_environment(curr_scene=curr_scene)
#Create targets
self.target_poses = self.check_targets(curr_scene, num_of_objects,
0.10)
rospy.sleep(5)
#Planner setup
self.man.set_planner_id("RRTConnectkConfigDefault")
self.man.set_num_planning_attempts(20)
self.man.allow_replanning(True)
#Pick and place every object
for i in xrange(len(self.target_poses)):
if i not in self.picked:
self.pick_object(curr_scene=curr_scene,
group=self.man,
part_index=i)
rospy.loginfo("END OF PICK AND PLACE")
if (len(self.picked) == len(self.target_poses)):
is_success = True
self.clear_poses()
return is_success
"""
----------------Function Name: execute_simulation----------------
Definition : Executes the simulation num_attempts times. Stores the success rate
and writes into the specified document
Num_Attempts : Limiting number for executing simulation. (Default Value: 10)
File_Name : File name (with path if needed) to write test results
"""
def test_simulation(self,
curr_scene=None,
num_attempts=10,
num_of_objects=3,
file_name="ur5_pick_place_test.txt"):
success_rate = 0
file = open(file_name, "a+")
file.write("Start Test \n")
for case in xrange(1, num_attempts + 1):
state = self.execute_simulation(curr_scene=curr_scene,
num_of_objects=num_of_objects)
if (state):
success_rate += 1
rospy.loginfo("Success Rate: " + str(success_rate) + "/" +
str(case))
file.write("Test Case %s is successfull" % str(case))
else:
rospy.loginfo("Success Rate: " + str(success_rate) + "/" +
str(case))
file.write("Test Case %s is failedl" % str(case))
file.write("Success: %" + str(((success_rate / (case - 1) * 100.0))))
file.write("----END OF TEST----")
file.close()
return
def graspobject():
# Connect to the right_arm move group
right_arm = MoveGroupCommander('manipulator')
right_arm.set_planner_id('RRTConnectkConfigDefault')
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame('base_link')
# Allow some leeway in position(meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.1)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
#right_arm.set_end_effector_link(end_effector_link)
#start_pose = Pose()
#start_pose.position.x = 0.587
#start_pose.position.y = 0.034
#start_pose.position.z = -0.010
#start_pose.orientation.x =0.508
#start_pose.orientation.y = 0.508
#start_pose.orientation.z = -0.491
#start_pose.orientation.w = 0.491
#right_arm.set_pose_target(start_pose)
#traj = right_arm.plan()
#rospy.sleep(2)
#right_arm.execute(traj)
#rospy.sleep(2)
global getkey
print '回到初始位置结束'
startclient = rospy.ServiceProxy('/detect_grasps/actionstart', npy)
startclient(0)
print 'call start over'
sub = rospy.Subscriber('/detect_grasps/clustered_grasps', GraspConfigList, graspcallback)
rate = rospy.Rate(200)
while not rospy.is_shutdown():
if getkey==1:
break
rate.sleep()
getkey=0
global grasps
print '检测到抓取位置，开始执行'
global ser
ser.write(('0\n').encode())
rospy.sleep(2)
print "-start movingA------------------------------------"
pose = []
currentcamera=current_cameralinkpose()
grasppose1,grasppose2= transformgrasp(currentcamera,grasps,0.24,0.15)
start_pose = current_pose()
print 'start_pose',start_pose
pose.append(start_pose)
pose.append(grasppose1)
pose.append(grasppose2)
#end_pose = copy.deepcopy(start_pose)
# end_pose.position.x = grasps.approach.x
# end_pose.position.y = grasps.approach.y
# end_pose.position.z = grasps.approach.z
catisian_moving(pose,right_arm)#迪卡尔插值到目标姿态
print "-start movingB------------------------------------"
print '抓'
ser.write(('1\n').encode())
rospy.sleep(2)
##############################################################
group_variable_values = right_arm.get_current_joint_values()
print "============ Joint values: ", group_variable_values
group_variable_values[0] =1.5740725994110107 #机器人的观测姿态定义在这里
group_variable_values[1] = -1.5415833632098597
group_variable_values[2] = -1.2562201658831995
group_variable_values[3] = -1.8575199286090296
group_variable_values[4] = 1.572489857673645
group_variable_values[5] = 1.5713907480239868
right_arm.set_joint_value_target(group_variable_values)
plan2 = right_arm.plan()
print "============ Waiting while RVIZ displays plan2..."
rospy.sleep(1)
right_arm.execute(plan2)#先移到观测姿态
group_variable_values = right_arm.get_current_joint_values()
print "============ Joint values: ", group_variable_values
group_variable_values[0] =2.6649599075317383#机器人的观测姿态定义在这里
group_variable_values[1] = -1.493981663380758
group_variable_values[2] = -1.7679532209979456
group_variable_values[3] =-1.3932693640338343
group_variable_values[4] = 1.5719022750854492
group_variable_values[5] = 1.5657243728637695
right_arm.set_joint_value_target(group_variable_values)
plan2 = right_arm.plan()
print "============ Waiting while RVIZ displays plan2..."
rospy.sleep(2)
right_arm.execute(plan2)#先移到观测姿态
print "-start movingB------------------------------------"
print '笛卡尔插值到放置位置完成'
ser.write(('0\n').encode())
rospy.sleep(2)
# pose = []
#posecurrent=current_pose()#获取现有的机器人末端姿态
# pose.append(posecurrent)
#start_pose = Pose()#似乎这里不能接受相对的移动
#start_pose= objectpose#获取目标末端姿态
# posecurrent.position.z =posecurrent.position.z -0.1
# pose.append(posecurrent)
# print start_pose
print "-------------------------------------"
# print pose
print "-------------------------------------"
# catisian_moving(pose,right_arm)#迪卡尔插值到目标姿态
#回到初始位置
group_variable_values = right_arm.get_current_joint_values()
print "============ Joint values: ", group_variable_values
group_variable_values[0] =1.5740725994110107 #机器人的观测姿态定义在这里
group_variable_values[1] = -1.5415833632098597
group_variable_values[2] = -1.2562201658831995
group_variable_values[3] = -1.8575199286090296
group_variable_values[4] = 1.572489857673645
group_variable_values[5] = 1.5713907480239868
right_arm.set_joint_value_target(group_variable_values)
plan2 = right_arm.plan()
print "============ Waiting while RVIZ displays plan2..."
rospy.sleep(1)
right_arm.execute(plan2)#先移到观测姿态
rospy.sleep(3)
print '回到初始位置结束'
startclient2 = rospy.ServiceProxy('/detect_grasps/actionover', npy)
startclient2(0)
print 'call start over'
# modify the pose
p.pose.position.x = 0.72
p.pose.position.z = 0.05
# add the pen
scene.add_mesh("pen",p,resourcepath+'objects/pen.dae')
rospy.sleep(1)
# print the existing groups
robot = RobotCommander()
print "Available groups: ",robot.get_group_names()
# setup the arm group and its planner
arm = MoveGroupCommander("arm")
arm.set_start_state_to_current_state()
arm.set_planner_id("RRTstarkConfigDefault")
arm.set_planning_time(5.0)
arm.detach_object("pen")
# set the arm to a safe target
arm.set_named_target("gamma")
# plan and execute the motion
arm.go()
# setup the hand group and its planner
hand = MoveGroupCommander("hand")
hand.set_start_state_to_current_state()
hand.set_planner_id("LBKPIECEkConfigDefault")
hand.set_planning_time(10.0)
# set the hand to a safe target
class SingleArmROS(SingleArmReal):
"""
A class for single arms with ROS-related functions.
Args:
cfgs (YACS CfgNode): configurations for the arm.
moveit_planner (str): motion planning algorithm.
eetool_cfg (dict): arguments to pass in the constructor
of the end effector tool class.
Attributes:
max_vel (float): Maximum joint velocity, read in from
MoveIt config file. Can be scaled with self.scale_motion().
max_acc (float): Maximum joint acceleration, read in from
MoveIt config file. Can be scaled with self.scale_motion().
tf_listner (tf.TransformListener): ROS subscriber to the /tf topic.
moveit_group (MoveGroupCommander): Internal interface to MoveIt!
move_group, used to call motion planning functions.
moveit_scene (MoveitScene): Internal interface to MoveIt! planning
scene, used to add objects, remove objects, etc. from the
motion planning environment.
moveit_planner (str): The name of the motion planning algorithm
being used for all motion planning requests.
"""
def __init__(self,
cfgs,
moveit_planner='RRTstarkConfigDefault',
eetool_cfg=None):
super(SingleArmROS, self).__init__(cfgs=cfgs, eetool_cfg=eetool_cfg)
self.moveit_planner = moveit_planner
self._gazebo_sim = rospy.get_param('sim')
self._init_ros_consts()
def set_jpos(self, position, joint_name=None, wait=True, *args, **kwargs):
"""
Method to send a joint position command to the robot (units in rad)
Args:
position (float or list or flattened np.ndarray):
desired joint position(s)
(shape: :math:`[DOF,]` if list, otherwise a single value).
joint_name (str): If not provided, position should be a list and
all actuated joints will be moved to specified positions. If
provided, only specified joint will move. Defaults to None.
wait (bool): whether position command should be blocking or non
blocking. Defaults to True.
Returns:
bool: True if command was completed successfully, returns
False if wait flag is set to False.
"""
position = copy.deepcopy(position)
if joint_name is None:
if len(position) != self.arm_dof:
raise ValueError('position should contain %d elements if '
'joint_name is not provided' % self.arm_dof)
tgt_pos = position
else:
if not isinstance(position, numbers.Number):
raise TypeError('position should be individual float value'
' if joint_name is provided')
if joint_name not in self.arm_jnt_names_set:
raise TypeError('Joint name [%s] is not in the arm'
' joint list!' % joint_name)
else:
tgt_pos = self.get_jpos()
arm_jnt_idx = self.arm_jnt_names.index(joint_name)
tgt_pos[arm_jnt_idx] = position
self.moveit_group.set_joint_value_target(tgt_pos)
success = self.moveit_group.go(tgt_pos, wait=wait)
return success
def set_jvel(self, velocity, joint_name=None, wait=False, *args, **kwargs):
"""
Set joint velocity command to the robot (units in rad/s).
Args:
velocity (float or list or flattened np.ndarray): list of target
joint velocity value(s)
(shape: :math:`[6,]` if list, otherwise a single value).
joint_name (str, optional): If not provided, velocity should be
list and all joints will be turned on at specified velocity.
Defaults to None.
wait (bool, optional): If True, block until robot reaches
desired joint velocity value(s). Defaults to False.
Returns:
bool: True if command was completed successfully, returns
False if wait flag is set to False.
"""
raise NotImplementedError
def set_ee_pose(self, pos=None, ori=None, wait=True, *args, **kwargs):
"""
Set cartesian space pose of end effector.
Args:
pos (list or np.ndarray): Desired x, y, z positions in the robot's
base frame to move to (shape: :math:`[3,]`).
ori (list or np.ndarray, optional): It can be euler angles
([roll, pitch, yaw], shape: :math:`[4,]`),
or quaternion ([qx, qy, qz, qw], shape: :math:`[4,]`),
or rotation matrix (shape: :math:`[3, 3]`). If it's None,
the solver will use the current end effector
orientation as the target orientation.
wait (bool): wait until the motion completes.
Returns:
bool: Returns True is robot successfully moves to goal pose.
"""
if ori is None and pos is None:
return True
if ori is None:
pose = self.get_ee_pose() # last index is euler angles
quat = pose[1]
else:
quat = arutil.to_quat(ori)
if pos is None:
pose = self.get_ee_pose()
pos = pose[0]
pose = self.moveit_group.get_current_pose()
pose.pose.position.x = pos[0]
pose.pose.position.y = pos[1]
pose.pose.position.z = pos[2]
pose.pose.orientation.x = quat[0]
pose.pose.orientation.y = quat[1]
pose.pose.orientation.z = quat[2]
pose.pose.orientation.w = quat[3]
self.moveit_group.set_pose_target(pose)
success = self.moveit_group.go(wait=wait)
return success
def move_ee_xyz(self,
delta_xyz,
eef_step=0.005,
wait=True,
*args,
**kwargs):
"""
Move end effector in straight line while maintaining orientation.
Args:
delta_xyz (list or np.ndarray): Goal change in x, y, z position of
end effector.
eef_step (float, optional): Discretization step in cartesian space
for computing waypoints along the path. Defaults to 0.005 (m).
wait (bool, optional): True if robot should not do anything else
until this goal is reached, or the robot times out.
Defaults to True.
Returns:
bool: True if robot successfully reached the goal pose.
"""
ee_pos, ee_quat, ee_rot_mat, ee_euler = self.get_ee_pose()
plan = moveit_cartesian_path(ee_pos, ee_quat, delta_xyz,
self.moveit_group, eef_step)
success = self.moveit_group.execute(plan, wait=wait)
return success
def get_jpos(self, joint_name=None):
"""
Gets the current joint position of the robot. Gets the value
from the internally updated dictionary that subscribes to the ROS
topic /joint_states.
Args:
joint_name (str, optional): If it's None,
it will return joint positions
of all the actuated joints. Otherwise, it will
return the joint position of the specified joint.
Returns:
One of the following
- float: joint position given joint_name.
- list: joint positions if joint_name is None
(shape: :math:`[DOF]`).
"""
self._j_state_lock.acquire()
if joint_name is not None:
if joint_name not in self.arm_jnt_names:
raise TypeError('Joint name [%s] '
'not recognized!' % joint_name)
jpos = self._j_pos[joint_name]
else:
jpos = []
for joint in self.arm_jnt_names:
jpos.append(self._j_pos[joint])
self._j_state_lock.release()
return jpos
def get_jvel(self, joint_name=None):
"""
Gets the current joint angular velocities of the robot. Gets the value
from the internally updated dictionary that subscribes to the ROS
topic /joint_states.
Args:
joint_name (str, optional): If it's None,
it will return joint velocities
of all the actuated joints. Otherwise, it will
return the joint position of the specified joint.
Returns:
One of the following
- float: joint velocity given joint_name.
- list: joint velocities if joint_name is None
(shape: :math:`[DOF]`).
"""
self._j_state_lock.acquire()
if joint_name is not None:
if joint_name not in self.arm_jnt_names:
raise TypeError('Joint name [%s] not recognized!' % joint_name)
jvel = self._j_vel[joint_name]
else:
jvel = []
for joint in self.arm_jnt_names:
jvel.append(self._j_vel[joint])
self._j_state_lock.release()
return jvel
def get_ee_pose(self):
"""
Get current cartesian pose of the EE, in the robot's base frame,
using ROS subscriber to the tf tree topic.
Returns:
4-element tuple containing
- np.ndarray: x, y, z position of the EE (shape: :math:`[3]`).
- np.ndarray: quaternion representation ([x, y, z, w]) of the EE
orientation (shape: :math:`[4]`).
- np.ndarray: rotation matrix representation of the EE orientation
(shape: :math:`[3, 3]`).
- np.ndarray: euler angle representation of the EE orientation
(roll, pitch, yaw with static reference frame)
(shape: :math:`[3]`).
"""
pos, quat = get_tf_transform(self.tf_listener,
self.cfgs.ARM.ROBOT_BASE_FRAME,
self.cfgs.ARM.ROBOT_EE_FRAME)
rot_mat = arutil.quat2rot(quat)
euler_ori = arutil.quat2euler(quat)
return np.array(pos), np.array(quat), rot_mat, euler_ori
def get_ee_vel(self):
"""
Return the end effector's velocity.
Returns:
2-element tuple containing
- np.ndarray: translational velocity (vx, vy, vz)
(shape: :math:`[3,]`).
- np.ndarray: rotational velocity
(wx, wy, wz) (shape: :math:`[3,]`).
"""
jpos = self.get_jpos()
jvel = self.get_jvel()
ee_vel = self.compute_fk_velocity(jpos, jvel,
self.cfgs.ARM.ROBOT_EE_FRAME)
return ee_vel[:3], ee_vel[3:]
def scale_motion(self, vel_scale=1.0, acc_scale=1.0):
"""
Sets the maximum velocity and acceleration for the robot
motion. Specified as a fraction
from 0.0 - 1.0 of the maximum velocity and acceleration
specified in the MoveIt joint limits configuration file.
Args:
vel_scale (float): velocity scale, Defaults to 1.0.
acc_scale (float): acceleration scale, Defaults to 1.0.
"""
vel_scale = arutil.clamp(vel_scale, 0.0, 1.0)
acc_scale = arutil.clamp(acc_scale, 0.0, 1.0)
self.moveit_group.set_max_velocity_scaling_factor(vel_scale)
self.moveit_group.set_max_acceleration_scaling_factor(acc_scale)
self._motion_vel = self.max_vel * vel_scale
self._motion_acc = self.max_acc * acc_scale
def _init_ros_consts(self):
"""
Initialize constants
"""
moveit_commander.roscpp_initialize(sys.argv)
self.moveit_group = MoveGroupCommander(self.cfgs.ARM.MOVEGROUP_NAME)
self.moveit_group.set_planner_id(self.moveit_planner)
self.moveit_group.set_planning_time(1.0)
self.moveit_scene = MoveitScene()
# read the joint limit (max velocity and acceleration) from the
# moveit configuration file
jnt_params = []
max_vels = []
max_accs = []
for arm_jnt in self.arm_jnt_names:
jnt_param = self.cfgs.ROBOT_DESCRIPTION + \
'_planning/joint_limits/' + arm_jnt
jnt_params.append(copy.deepcopy(jnt_param))
max_vels.append(rospy.get_param(jnt_param + '/max_velocity'))
max_accs.append(rospy.get_param(jnt_param + '/max_acceleration'))
self.max_vel = np.min(max_vels)
self.max_acc = np.min(max_accs)
self._j_pos = dict()
self._j_vel = dict()
self._j_torq = dict()
self._j_state_lock = threading.RLock()
self.tf_listener = tf.TransformListener()
rospy.Subscriber(self.cfgs.ARM.ROSTOPIC_JOINT_STATES, JointState,
self._callback_joint_states)
def _callback_joint_states(self, msg):
"""
ROS subscriber callback for arm joint states
Args:
msg (sensor_msgs/JointState): Contains message published in topic.
"""
self._j_state_lock.acquire()
for idx, name in enumerate(msg.name):
if name in self.arm_jnt_names:
if idx < len(msg.position):
self._j_pos[name] = msg.position[idx]
if idx < len(msg.velocity):
self._j_vel[name] = msg.velocity[idx]
if idx < len(msg.effort):
self._j_torq[name] = msg.effort[idx]
self._j_state_lock.release()
class Arm():
def __init__(self):
self.p = Pose()
self.gripper = MoveGroupCommander("onine_gripper")
self.arm = MoveGroupCommander("onine_arm")
self.arm.set_goal_tolerance(0.004)
self.arm.allow_replanning(True)
# self.arm.set_goal_position_tolerance(0.005)
# self.arm.set_goal_orientation_tolerance(0.1)
self.arm.set_num_planning_attempts(10)
self.arm.set_planning_time(5)
self.arm.set_planner_id("RRTkConfigDefault")
def go(self, x, y, z, roll, pitch, yaw):
self.p.position.x = x
self.p.position.y = y
self.p.position.z = z
self.p.orientation = Quaternion(
*quaternion_from_euler(roll, pitch, yaw))
self.arm.set_pose_target(self.p)
os.system("rosservice call clear_octomap")
rospy.loginfo("Moving to arm target")
self.arm.go(wait=True)
rospy.sleep(1)
def get_valid_pose(self, x, y, z, distance):
origin_translation = [0.095, 0.00, 0.00]
delta_x = x - origin_translation[0]
delta_y = y - origin_translation[1]
theta = math.atan(delta_y / delta_x)
grasp_yaw = theta
grasp_x = x + (distance * math.cos(theta))
grasp_y = y + (distance * math.sin(theta))
return (grasp_x, grasp_y, z, grasp_yaw)
def ready(self):
self.arm.set_named_target("onine_ready")
self.arm.go(wait=True)
def home(self):
self.arm.set_named_target("onine_home")
self.arm.go(wait=True)
def feed_pos(self):
self.arm.set_named_target("feed_pos")
self.arm.go(wait=True)
def tilt_food(self):
self.arm.set_named_target("tilt_food")
self.arm.go(wait=True)
def open_gripper(self):
os.system("rostopic pub /onine_gripper std_msgs/Float64 0.085 -1")
os.system("rosservice call clear_octomap")
def close_gripper(self):
os.system("rostopic pub /onine_gripper std_msgs/Float64 0.035 -1")
os.system("rosservice call clear_octomap")
def pickup_sim(self, x, y, z):
# self.ready()
self.open_gripper()
(aim_x, aim_y, aim_z,
aim_yaw) = self.get_valid_pose(x, y, z + 0.15, 0.00)
self.go(aim_x, aim_y, aim_z, 0.0, 0.0, aim_yaw)
(aim_x, aim_y, aim_z, aim_yaw) = self.get_valid_pose(x, y, z, 0.00)
self.go(aim_x, aim_y, aim_z, 0.0, 0.0, aim_yaw)
self.close_gripper()
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
right_arm.set_planner_id("KPIECEkConfigDefault")
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
eef = right_arm.get_end_effector_link()
rospy.sleep(2)
scene.remove_attached_object(GRIPPER_FRAME, "part")
# clean the scene
scene.remove_world_object("table")
scene.remove_world_object("part")
#right_arm.set_named_target("r_start")
#right_arm.go()
#right_gripper.set_named_target("right_gripper_open")
#right_gripper.go()
rospy.sleep(1)
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
# add a table
#p.pose.position.x = 0.42
#p.pose.position.y = -0.2
#p.pose.position.z = 0.3
#scene.add_box("table", p, (0.5, 1.5, 0.6))
# add an object to be grasped
p.pose.position.x = 0.40
p.pose.position.y = -0.0
p.pose.position.z = 0.85
scene.add_box("part", p, (0.07, 0.01, 0.2))
rospy.sleep(1)
g = Grasp()
g.id = "test"
start_pose = PoseStamped()
start_pose.header.frame_id = FIXED_FRAME
# start the gripper in a neutral pose part way to the target
start_pose.pose.position.x = 0.37636
start_pose.pose.position.y = 0.21886
start_pose.pose.position.z = 0.9
start_pose.pose.orientation.x = 0.00080331
start_pose.pose.orientation.y = 0.001589
start_pose.pose.orientation.z = -2.4165e-06
start_pose.pose.orientation.w = 1
right_arm.set_pose_target(start_pose)
right_arm.go()
rospy.sleep(2)
# generate a list of grasps
grasps = self.make_grasps(start_pose)
result = False
n_attempts = 0
# repeat until will succeed
while result == False:
result = robot.right_arm.pick("part", grasps)
n_attempts += 1
print "Attempts: ", n_attempts
rospy.sleep(0.3)
rospy.spin()
roscpp_shutdown()
class MoveItObstaclesDemo:
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_obstacles_demo')
# 等待场景准备就绪
rospy.sleep(1)
# 初始化需要使用move group控制的机械臂中的arm group
self.arm = MoveGroupCommander('manipulator')
# 设置机械臂工作空间
self.arm.set_workspace([-100, -100, 0, 100, 0.3, 100])
# 设置机械臂最大速度
self.arm.set_max_velocity_scaling_factor(value=0.1)
# 获取终端link的名称
self.end_effector_link = self.arm.get_end_effector_link()
rospy.loginfo('end effector link: {}'.format(self.end_effector_link))
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
self.arm.set_goal_position_tolerance(0.01)
self.arm.set_goal_orientation_tolerance(0.05)
# 当运动规划失败后，允许重新规划
# self.arm.allow_replanning(True)
self.arm.set_num_planning_attempts(10)
# self.arm.allow_looking(True)
# 设置目标位置所使用的参考坐标系
self.reference_frame = 'base_link'
self.arm.set_pose_reference_frame(self.reference_frame)
# 设置每次运动规划的时间限制：5s
self.arm.set_planning_time(3)
# 控制机械臂先回到初始化位置
self.arm.set_named_target('home')
self.arm.go()
rospy.sleep(2)
def planning(self, start_point, end_point):
"""
功能：动态避障
:param start_point: 起始点, type: dict
:param end_point: 终点, type: dict
:return: None
"""
# 移动机械臂到指定位置，并获取当前位姿数据为机械臂运动的起始位姿
if start_point:
self.move_arm(p=start_point)
self.start_pose = self.arm.get_current_pose(self.end_effector_link).pose
# 使用moveit自带的求解器规划出A到B的离散路径点, 并存到列表way_points中
# way_points = self.get_way_points(start_point, end_point)
while True:
print('set planner id: RRT')
self.arm.set_planner_id('TRRTkConfigDefault')
self.arm.set_named_target('up')
self.arm.go()
rospy.sleep(5)
print('set planner id: PRM')
self.arm.set_planner_id('TRRTkConfigDefault')
self.arm.set_named_target('home')
self.arm.go()
rospy.sleep(5)
# -------------------------------------------------------------------
def get_way_points(self, a, b):
way_points = []
# plan 1
self.arm.set_named_target('up')
traj = self.arm.plan()
if traj.joint_trajectory.points: # True if trajectory contains points
rospy.loginfo("get trajectory success")
else:
rospy.logerr("Trajectory is empty. Planning false!")
self.arm.clear_pose_targets()
# plan 2
# traj = self.moveit_planning(p=b)
for i, p in enumerate(traj.joint_trajectory.points):
# rospy.loginfo('waypoint: {}'.format(i))
if i%2 == 0:
point = {
'x': p.positions[0],
'y': p.positions[1],
'z': p.positions[2],
'ox': p.positions[3],
'oy': p.positions[4],
'oz': p.positions[5]
}
way_points.append(point)
rospy.loginfo('waypoint: \n {}'.format(way_points))
rospy.loginfo(len(way_points))
return way_points
def moveit_planning(self, p):
"""
使用moveit求解器规划路径
:param p: dict, e.g., {'x': 0, 'y': 0, 'z': 0, 'ox': 0, 'oy': 0, 'oz': 0, 'ow': 0}
:return:
"""
rospy.loginfo('start moveit planning...')
target_pose = PoseStamped()
target_pose.header.frame_id = self.reference_frame
target_pose.pose.position.x = p['x']
target_pose.pose.position.y = p['y']
target_pose.pose.position.z = p['z']
if 'ox' in p.keys() and p['ox']:
target_pose.pose.orientation.x = p['ox']
if 'oy' in p.keys() and p['oy']:
target_pose.pose.orientation.y = p['oy']
if 'oz' in p.keys() and p['oz']:
target_pose.pose.orientation.z = p['oz']
if 'ow' in p.keys() and p['ow']:
target_pose.pose.orientation.w = p['ow']
# 传入一个PoseStamped
# self.arm.set_pose_target(target_pose, self.end_effector_link)
# 尝试直接传入一个列表
self.arm.set_pose_target([p['x'], p['y'], p['z'], p['ox'], p['oy'], p['oz']], self.end_effector_link)
traj = self.arm.plan()
if traj.joint_trajectory.points: # True if trajectory contains points
rospy.loginfo("get trajectory success")
return traj
else:
rospy.logerr("Trajectory is empty. Planning false!")
def move_arm(self, p):
"""
移动机械臂到p点
:param p: dict, e.g., {'x': 0, 'y': 0, 'z': 0, 'ox': 0, 'oy': 0, 'oz': 0, 'ow': 0}
:return:
"""
rospy.loginfo('start arm moving...')
traj = self.moveit_planning(p)
self.arm.execute(traj)
# 设置当前位置为起始位置
self.arm.set_start_state_to_current_state()
rospy.sleep(1)
def stop_arm(self):
"""
急停
:return:
"""
pass
def exist_danger_obstacle(self):
"""
环境中是否存在危险的障碍物
:return: True or False
"""
return False
def get_obstacle_octomap(self):
"""
获取环境的octomap信息
:return:
"""
pass
def __init__(self):
# Initialize the move_group API
print 'starting'
moveit_commander.roscpp_initialize(sys.argv)
# Connect to the right_arm move group
right_arm = MoveGroupCommander('manipulator')
right_arm.set_planner_id('RRTConnectkConfigDefault')
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame('base_link')
# Allow some leeway in position(meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.1)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
#right_arm.set_end_effector_link(end_effector_link)
# Get the current pose so we can add it as a waypoint
group_variable_values = right_arm.get_current_joint_values()
print "============ Joint values: ", group_variable_values
group_variable_values[0] =1.5740725994110107 #机器人的观测姿态定义在这里
group_variable_values[1] = -1.5415833632098597
group_variable_values[2] = -1.2562201658831995
group_variable_values[3] = -1.8575199286090296
group_variable_values[4] = 1.572489857673645
group_variable_values[5] = 1.5713907480239868
right_arm.set_joint_value_target(group_variable_values)
plan2 = right_arm.plan()
print "============ Waiting while RVIZ displays plan2..."
rospy.sleep(2)
right_arm.execute(plan2)#先移到观测姿态
try:
rate = rospy.Rate(10)
while not rospy.is_shutdown():
print '.'
graspobject( )
rate.sleep()
except rospy.ROSInterruptException:
pass
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node("moveit_py_demo", anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
right_arm.set_planner_id("KPIECEkConfigDefault")
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
eef = right_arm.get_end_effector_link()
rospy.sleep(2)
scene.remove_attached_object(GRIPPER_FRAME, "part")
# clean the scene
scene.remove_world_object("table")
scene.remove_world_object("part")
# right_arm.set_named_target("r_start")
# right_arm.go()
# right_gripper.set_named_target("right_gripper_open")
# right_gripper.go()
rospy.sleep(1)
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
# add a table
# p.pose.position.x = 0.42
# p.pose.position.y = -0.2
# p.pose.position.z = 0.3
# scene.add_box("table", p, (0.5, 1.5, 0.6))
# add an object to be grasped
p.pose.position.x = 0.40
p.pose.position.y = -0.0
p.pose.position.z = 0.85
scene.add_box("part", p, (0.07, 0.01, 0.2))
rospy.sleep(1)
g = Grasp()
g.id = "test"
start_pose = PoseStamped()
start_pose.header.frame_id = FIXED_FRAME
# start the gripper in a neutral pose part way to the target
start_pose.pose.position.x = 0.37636
start_pose.pose.position.y = 0.21886
start_pose.pose.position.z = 0.9
start_pose.pose.orientation.x = 0.00080331
start_pose.pose.orientation.y = 0.001589
start_pose.pose.orientation.z = -2.4165e-06
start_pose.pose.orientation.w = 1
right_arm.set_pose_target(start_pose)
right_arm.go()
rospy.sleep(2)
# generate a list of grasps
grasps = self.make_grasps(start_pose)
result = False
n_attempts = 0
# repeat until will succeed
while result == False:
result = robot.right_arm.pick("part", grasps)
n_attempts += 1
print "Attempts: ", n_attempts
rospy.sleep(0.3)
rospy.spin()
roscpp_shutdown()
group.set_path_constraints(constraints)
rospy.loginfo("Get Path Constraints:\n{}".format(
group.get_path_constraints()))
# Pose Target 2
rospy.loginfo("Start Pose Target 2")
pose_target_2 = Pose()
pose_target_2.position.x = 0.3
pose_target_2.position.y = -0.5
pose_target_2.position.z = 0.15
pose_target_2.orientation.x = 0.0
pose_target_2.orientation.y = -0.707
pose_target_2.orientation.z = 0.0
pose_target_2.orientation.w = 0.707
group.set_planner_id("RRTConnectkConfigDefault")
group.allow_replanning(True)
rospy.loginfo("Set Target to Pose:\n{}".format(pose_target_2))
xyz = [
pose_target_2.position.x, pose_target_2.position.y,
pose_target_2.position.z
]
group.set_position_target(xyz)
result_p = group.go()
rospy.loginfo("Moving to the Position Executed... {}".format(result_p))
group.clear_path_constraints()
class TestPlanners(object):
def __init__(self, group_id, planner):
rospy.init_node('moveit_test_planners', anonymous=True)
self.pass_list = []
self.fail_list = []
self.planner = planner
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
self.group = MoveGroupCommander(group_id)
rospy.sleep(1)
self.group.set_planner_id(self.planner)
self.test_trajectories_empty_environment()
self.test_waypoints()
self.test_trajectories_with_walls_and_ground()
for pass_test in self.pass_list:
print(pass_test)
for fail_test in self.fail_list:
print(fail_test)
def _add_walls_and_ground(self):
# publish a scene
p = geometry_msgs.msg.PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.pose.position.x = 0
p.pose.position.y = 0
# offset such that the box is below ground (to prevent collision with
# the robot itself)
p.pose.position.z = -0.11
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 1
self.scene.add_box("ground", p, (3, 3, 0.1))
p.pose.position.x = 0.4
p.pose.position.y = 0.85
p.pose.position.z = 0.4
p.pose.orientation.x = 0.5
p.pose.orientation.y = -0.5
p.pose.orientation.z = 0.5
p.pose.orientation.w = 0.5
self.scene.add_box("wall_front", p, (0.8, 2, 0.01))
p.pose.position.x = 1.33
p.pose.position.y = 0.4
p.pose.position.z = 0.4
p.pose.orientation.x = 0.0
p.pose.orientation.y = -0.707388
p.pose.orientation.z = 0.0
p.pose.orientation.w = 0.706825
self.scene.add_box("wall_right", p, (0.8, 2, 0.01))
p.pose.position.x = -0.5
p.pose.position.y = 0.4
p.pose.position.z = 0.4
p.pose.orientation.x = 0.0
p.pose.orientation.y = -0.707107
p.pose.orientation.z = 0.0
p.pose.orientation.w = 0.707107
self.scene.add_box("wall_left", p, (0.8, 2, 0.01))
# rospy.sleep(1)
def _check_plan(self, plan):
if len(plan.joint_trajectory.points) > 0:
return True
else:
return False
def _plan_joints(self, joints):
self.group.clear_pose_targets()
group_variable_values = self.group.get_current_joint_values()
group_variable_values[0:6] = joints[0:6]
self.group.set_joint_value_target(group_variable_values)
plan = self.group.plan()
return self._check_plan(plan)
def test_trajectories_rotating_each_joint(self):
# test_joint_values = [numpy.pi/2.0, numpy.pi-0.33, -numpy.pi/2]
test_joint_values = [numpy.pi / 2.0]
joints = [0.0, 0.0, 0.0, -numpy.pi / 2.0, 0.0, 0.0]
# Joint 4th is colliding with the hand
# for joint in range(6):
for joint in [0, 1, 2, 3, 5]:
for value in test_joint_values:
joints[joint] = value
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_rotating_each_joint, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_rotating_each_joint, " +
self.planner + ", joints:" + str(joints))
def test_trajectories_empty_environment(self):
# Up - Does not work with sbpl but it does with ompl
joints = [0.0, -1.99, 2.19, 0.58, 0.0, -0.79, 0.0, 0.0]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# All joints up
joints = [
-1.67232, -2.39104, 0.264862, 0.43346, 2.44148, 2.48026, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# Down
joints = [
-0.000348431194526, 0.397651011661, 0.0766181197394,
-0.600353691727, -0.000441966540076, 0.12612019707, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# left
joints = [
0.146182953165, -2.6791929848, -0.602721109682, -3.00575848765,
0.146075718452, 0.00420656698366, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# Front
joints = [
1.425279839, -0.110370375874, -1.52548746261, -1.50659865247,
-1.42700242769, 3.1415450794, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# Behind
joints = [
1.57542451065, 3.01734161219, 2.01043257686, -1.14647092839,
0.694689321451, -0.390769365032, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# Should fail because it is in self-collision
joints = [
-0.289797803762, 2.37263860495, 2.69118483159, 1.65486712181,
1.04235601797, -1.69730925867, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
def test_waypoints(self):
# Start planning in a given joint position
joints = [
-0.324590029242, -1.42602359749, -1.02523472017, -0.754761892979,
0.344227622185, -3.03250264451, 0.0, 0.0
]
current = RobotState()
current.joint_state.name = self.robot.get_current_state(
).joint_state.name
current_joints = list(
self.robot.get_current_state().joint_state.position)
current_joints[0:8] = joints
current.joint_state.position = current_joints
self.group.set_start_state(current)
waypoints = []
initial_pose = self.group.get_current_pose().pose
initial_pose.position.x = -0.301185959729
initial_pose.position.y = 0.517069787724
initial_pose.position.z = 1.20681710541
initial_pose.orientation.x = 0.0207499700474
initial_pose.orientation.y = -0.723943002716
initial_pose.orientation.z = -0.689528413407
initial_pose.orientation.w = 0.00515118111221
# start with a specific position
waypoints.append(initial_pose)
# first move it down
wpose = geometry_msgs.msg.Pose()
wpose.orientation = waypoints[0].orientation
wpose.position.x = waypoints[0].position.x
wpose.position.y = waypoints[0].position.y
wpose.position.z = waypoints[0].position.z - 0.20
waypoints.append(copy.deepcopy(wpose))
# second front
wpose.position.y += 0.20
waypoints.append(copy.deepcopy(wpose))
# third side
wpose.position.x -= 0.20
waypoints.append(copy.deepcopy(wpose))
# fourth return to initial pose
wpose = waypoints[0]
waypoints.append(copy.deepcopy(wpose))
(plan3,
fraction) = self.group.compute_cartesian_path(waypoints, 0.01, 0.0)
if not self._check_plan(plan3) and fraction > 0.8:
self.fail_list.append("Failed: test_waypoints, " + self.planner)
else:
self.pass_list.append("Passed: test_waypoints, " + self.planner)
def test_trajectories_with_walls_and_ground(self):
self._add_walls_and_ground()
# Should fail to plan: Goal is in collision with the wall_front
joints = [
0.302173213174, 0.192487443763, -1.94298265002, 1.74920382275,
0.302143499777, 0.00130280337897, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
# Should fail to plan: Goal is in collision with the ground
joints = [
3.84825722288e-05, 0.643694953509, -1.14391175311, 1.09463824437,
0.000133883149666, -0.594498939239, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
# Goal close to right corner
joints = [
0.354696232081, -0.982224980654, 0.908055961723, -1.92328051116,
-1.3516255551, 2.8225061435, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed, test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
self.scene.remove_world_object("ground")
self.scene.remove_world_object("wall_left")
self.scene.remove_world_object("wall_right")
self.scene.remove_world_object("wall_front")
def __init__(self, pickPos, placePos):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# rospy.init_node('moveit_demo')
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the move group for the right arm
pick_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the momitive group for the right gripper
pick_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = pick_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
pick_arm.set_goal_position_tolerance(0.05)
pick_arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
pick_arm.allow_replanning(True)
# Set the right arm reference frame
pick_arm.set_pose_reference_frame(REFERENCE_FRAME)
#pick_arm.set_planner_id("RRTConnectkConfigDefault")
#pick_arm.set_planner_id("RRTstarkConfigDefault")
pick_arm.set_planner_id("RRTstarkConfigDefault")
# Allow 5 seconds per planning attempt
pick_arm.set_planning_time(3)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 5
# Set a limit on the number of place attempts
max_place_attempts = 5
# Give the scene a chance to catch up
rospy.sleep(2)
# Give each of the scene objects a unique name
base_table_id = 'base_table'
target_id = 'target'
limit_table_id = 'limit_table'
#tool_id = 'tool'
# Remove leftover objects from a previous run
scene.remove_world_object(base_table_id)
#scene.remove_world_object(table_id)
#scene.remove_world_object(box1_id)
#scene.remove_world_object(box2_id)
scene.remove_world_object(target_id)
#scene.remove_world_object(tool_id)
scene.remove_world_object(limit_table_id)
# Remove any attached objects from a previous session
scene.remove_attached_object(GRIPPER_FRAME, target_id)
# Open the gripper to the neutral position
# pick_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
# pick_gripper.go()
rospy.sleep(1)
# Set the height of the table off the ground
# table_ground = 0.6
table_ground = 0.0
# Set the dimensions of the scene objects [l, w, h]
base_table_size = [2, 2, 0.04]
#table_size = [0.2, 0.7, 0.01]
#box1_size = [0.1, 0.05, 0.05]
#box2_size = [0.05, 0.05, 0.15]
# Set the target size [l, w, h]
target_size = [0.055, 0.055, 0.12]
target_radius = 0.03305
target_height = 0.12310
limit_table_size = [0.6, 1.8, 0.04]
# Add a base table to the scene
base_table_pose = PoseStamped()
base_table_pose.header.frame_id = REFERENCE_FRAME
base_table_pose.pose.position.x = 0.0
base_table_pose.pose.position.y = 0.0
base_table_pose.pose.position.z = -0.3
base_table_pose.pose.orientation.w = 1.0
scene.add_box(base_table_id, base_table_pose, base_table_size)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
pick_arm.set_named_target(ARM_HOME_POSE)
pick_arm.go()
rospy.sleep(1)
# Add a table top and two boxes to the scene
#table_pose = PoseStamped()
#table_pose.header.frame_id = REFERENCE_FRAME
#table_pose.pose.position.x = 0.5
#table_pose.pose.position.y = -0.4
#table_pose.pose.position.z = table_ground + table_size[2] / 2.0
#table_pose.pose.orientation.w = 1.0
#scene.add_box(table_id, table_pose, table_size)
# Set the target pose in between the boxes and on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = pickPos.pose.position.x
target_pose.pose.position.y = pickPos.pose.position.y
target_pose.pose.position.z = pickPos.pose.position.z
target_pose.pose.orientation.w = pickPos.pose.orientation.w
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
#scene.add_cylinder(target_id,target_pose,target_height,target_radius)
#Add the limit_table object to the scene
limit_table_pose = PoseStamped()
limit_table_pose.header.frame_id = REFERENCE_FRAME
limit_table_pose.pose.position.x = 0.58
limit_table_pose.pose.position.y = -0.4
limit_table_pose.pose.position.z = 0.18
limit_table_pose.pose.orientation.w = 1.0
#scene.add_box(limit_table_id, limit_table_pose, limit_table_size)
self.setColor(limit_table_id, 0.6, 0.2, 0.2, 1.0)
# Make the table red and the boxes orange
#self.setColor(table_id, 0.8, 0, 0, 1.0)
# self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
# self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# Make the target yellow
self.setColor(target_id, 0.9, 0.9, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the support surface name to the table object
#pick_arm.set_support_surface_name(table_id)
# Specify a pose to place the target after being picked up
#指定拾取后的放置目标姿态
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = placePos.pose.position.x
place_pose.pose.position.y = placePos.pose.position.y
place_pose.pose.position.z = placePos.pose.position.z
place_pose.pose.orientation.w = placePos.pose.orientation.w
place_pose.pose.orientation.x = placePos.pose.orientation.x
place_pose.pose.orientation.y = placePos.pose.orientation.y
place_pose.pose.orientation.z = placePos.pose.orientation.z
#设置机器人运行最大速度位百分之～
pick_arm.set_max_velocity_scaling_factor(0.5)
# Initialize the grasp pose to the target pose
#初始化抓取目标点位
grasp_pose = target_pose
# Shift the grasp pose by half the width of the target to center it
# this way is just used by PR2 robot
#grasp_pose.pose.position.y -= target_size[1] / 2.0
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id])
# Publish the grasp poses so they can be viewed in RV
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# Track success/failure and number of attempts for pick operation
result = None
n_attempts = 0
# Repeat until we succeed or run out of attempts
#重复直到成功或者超出尝试的次数
#while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
while result != 1 and n_attempts < max_pick_attempts:
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
#moveit中的pick接口，target_id为moveit添加场景的id，此处为目标物体，grasps为可尝试抓取的点位序列
result = pick_arm.pick(target_id, grasps)
#打印信息
rospy.logerr("pick_arm.pick： " + str(result))
rospy.sleep(0.2)
# If the pick was successful, attempt the place operation
#如果抓取成功，尝试放置操作
result_g = None
n_attempts = 2
#if result == MoveItErrorCodes.SUCCESS:
while result_g != True and n_attempts < max_pick_attempts: #and result == 1:
# Generate valid place poses
#places = self.make_places(place_pose)
n_attempts += 1
print("-------------------")
print(place_pose)
#更新当前的机械臂状态
pick_arm.set_start_state_to_current_state()
#设置moveit运动的目标点位
pick_arm.set_pose_target(place_pose)
# Repeat until we succeed or run out of attempts
#while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
# n_attempts += 1
# rospy.loginfo("Place attempt: " + str(n_attempts))
# for place in places:
# result = pick_arm.place(target_id, place)
# if result == MoveItErrorCodes.SUCCESS:
# break
# rospy.sleep(0.2)
#moveit的运动接口，wait=True表示等到执行完成才返回
result_g = pick_arm.go(wait=True)
rospy.logerr("pick_arm.go： " + str(result_g))
#打开夹爪
open_client(500)
rospy.sleep(0.2)
#if result == "False"
# rospy.logerr("Place operation failed after " + str(n_attempts) + " attempts.")
#else:
# rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.")
# Return the arm to the "home" pose stored in the SRDF file
#将机械臂返回到SRDF中的“home”姿态
#scene.remove_world_object(target_id)
#scene.remove_attached_object(GRIPPER_FRAME, target_id)
#scene.remove_world_object(target_id)
rospy.sleep(1)
open_client(500)
pick_arm.set_named_target(ARM_HOME_POSE)
pick_arm.go()
# Open the gripper to the neutral position
# pick_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
# pick_gripper.go()
rospy.sleep(1)
rospy.logerr("pick_server over ")
msg = Float64()
msg.data = -1.0
for i in range(30):
pub.publish(msg)
rospy.sleep(0.1)
if __name__ == '__main__':
rospy.init_node('task_2_cheat', anonymous=True)
# for for 5 sec
rospy.sleep(5)
rospy.loginfo("start program %f" % rospy.get_time())
arm = MoveGroupCommander("ur5_arm")
arm.set_planner_id('RRTConnectkConfigDefault')
pub = rospy.Publisher("/r_gripper_controller/command",
Float64,
queue_size=1)
client = actionlib.SimpleActionClient(
'arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
client.wait_for_server()
rospy.loginfo("init pose")
msg = FollowJointTrajectoryGoal()
msg.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.2)
msg.trajectory.joint_names = [
'ur5_arm_shoulder_pan_joint', 'ur5_arm_shoulder_lift_joint',
'ur5_arm_elbow_joint', 'ur5_arm_wrist_1_joint',
'ur5_arm_wrist_2_joint', 'ur5_arm_wrist_3_joint'
]
def close_hand():
msg = Float64()
msg.data = -1.0
for i in range(30):
pub.publish(msg)
rospy.sleep(0.1)
if __name__ == '__main__':
rospy.init_node('task_2_cheat', anonymous=True)
# for for 5 sec
rospy.sleep(5)
rospy.loginfo("start program %f" % rospy.get_time())
arm = MoveGroupCommander("ur5_arm")
arm.set_planner_id('RRTConnectkConfigDefault')
pub = rospy.Publisher("/r_gripper_controller/command", Float64, queue_size=1)
client = actionlib.SimpleActionClient('arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
client.wait_for_server()
rospy.loginfo("init pose")
msg = FollowJointTrajectoryGoal()
msg.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.2)
msg.trajectory.joint_names = ['ur5_arm_shoulder_pan_joint',
'ur5_arm_shoulder_lift_joint',
'ur5_arm_elbow_joint',
'ur5_arm_wrist_1_joint',
'ur5_arm_wrist_2_joint',
'ur5_arm_wrist_3_joint']
msg.trajectory.points.append(JointTrajectoryPoint(positions=[-1.57, -0.1745, -2.79, -1.57, 0, 0],
time_from_start=rospy.Duration(2)))
from moveit_commander import RobotCommander, MoveGroupCommander
from moveit_commander import PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
from geometry_msgs.msg import PoseStamped
from moveit_msgs.msg import Grasp, GripperTranslation, PlaceLocation
from trajectory_msgs.msg import JointTrajectoryPoint
if __name__ == '__main__':
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
GRIPPER_FRAME = 'right_gripper_link'
scene = PlanningSceneInterface()
robot = RobotCommander()
right_arm = MoveGroupCommander("right_arm")
right_gripper = MoveGroupCommander("right_gripper")
right_arm.set_planner_id("KPIECEkConfigDefault")
rospy.sleep(1)
# clean the scene
scene.remove_world_object("table")
scene.remove_world_object("part")
scene.remove_attached_object(GRIPPER_FRAME, "part")
#rospy.logwarn("cleaning world")
#right_arm.set_named_target("r_start")
#right_arm.go()
#right_gripper.set_named_target("open")
#right_gripper.go()
rospy.sleep(3)
from geometry_msgs.msg import PoseStamped
import moveit_msgs.msg
import geometry_msgs.msg
import moveit_commander
if __name__=='__main__':
roscpp_initialize(sys.argv)
rospy.init_node('move_demo', anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
group = MoveGroupCommander("manipulator")
# specify the planner
group.set_planner_id("RRTkConfigDefault")
rospy.sleep(3)
## We create this DisplayTrajectory publisher which is used below to publish
## trajectories for RVIZ to visualize.
# display_trajectory_publisher = rospy.Publisher(
# '/move_group/display_planned_path',
# moveit_msgs.msg.DisplayTrajectory)
print ">>>>> remove scenes"
# clean the scene
scene.remove_world_object("pole")
scene.remove_world_object("ball")
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)
# Initialize the move group for the right arm
Arm = MoveGroupCommander(GROUP_NAME_ARM)
Arm.set_planner_id("RRTstarkConfigDefault")
# Initialize the move group for the right gripper
Hand = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = Arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
Arm.set_goal_position_tolerance(0.05)
Arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
Arm.allow_replanning(True)
# Set the right arm reference frame
Arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
Arm.set_planning_time(10)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 3
# Set a limit on the number of place attempts
max_place_attempts = 3
# Give the scene a chance to catch up
rospy.sleep(2)
# Give each of the scene objects a unique name
target_id = 'target'
# Remove leftover objects from a previous run
scene.remove_world_object(target_id)
# Remove any attached objects from a previous session
scene.remove_attached_object(GRIPPER_FRAME, target_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Open the gripper to the neutral position
#Hand.set_joint_value_target(GRIPPER_NEUTRAL)
#Hand.go()
#rospy.sleep(1)
target_size = [0.01, 0.01, 0.01]
# Set the target pose in between the boxes and on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.3
target_pose.pose.position.y = 0.03
target_pose.pose.position.z = -0.3
target_pose.pose.orientation.w = 1.0
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
# Initialize the grasp pose to the target pose
grasp_pose = PoseStamped()
grasp_pose.header.frame_id = REFERENCE_FRAME
grasp_pose.pose = target_pose.pose
grasp_pose.pose.position.y = 0.2
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id])
# Publish the grasp poses so they can be viewed in RViz
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# Track success/failure and number of attempts for pick operation
result = None
n_attempts = 0
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
result = Arm.pick(target_id, grasps)
rospy.sleep(0.2)
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
class CalibrationMovements:
def __init__(self,
move_group_name,
max_velocity_scaling,
max_acceleration_scaling,
angle_delta,
translation_delta,
move_group_namespace='/'):
# self.client = HandeyeClient() # TODO: move around marker when eye_on_hand, automatically take samples via trigger topic
if not move_group_namespace.endswith('/'):
move_group_namespace = move_group_namespace + '/'
if move_group_namespace != '/':
self.mgc = MoveGroupCommander(
move_group_name,
robot_description=move_group_namespace + 'robot_description',
ns=move_group_namespace)
else:
self.mgc = MoveGroupCommander(move_group_name)
self.mgc.set_planner_id("RRTConnectkConfigDefault"
) # TODO: this is only needed for the UR5
self.mgc.set_max_velocity_scaling_factor(max_velocity_scaling)
self.mgc.set_max_acceleration_scaling_factor(max_acceleration_scaling)
self.start_pose = self.mgc.get_current_pose()
self.joint_limits = [math.radians(90)] * 5 + [math.radians(180)] + [
math.radians(350)
] # TODO: make param
self.target_poses = []
self.current_pose_index = None
self.current_plan = None
self.fallback_joint_limits = [math.radians(90)] * 4 + [
math.radians(90)
] + [math.radians(180)] + [math.radians(350)]
if len(self.mgc.get_active_joints()) == 6:
self.fallback_joint_limits = self.fallback_joint_limits[1:]
self.angle_delta = angle_delta
self.translation_delta = translation_delta
def set_and_check_starting_position(self):
# sets the starting position to the current robot cartesian EE position and checks movement in all directions
# TODO: make repeatable
# - save the home position and each target position as joint position
# - plan to each target position and back, save plan if not crazy
# - return true if can plan to each target position without going crazy
self.start_pose = self.mgc.get_current_pose()
self.target_poses = self._compute_poses_around_state(
self.start_pose, self.angle_delta, self.translation_delta)
self.current_pose_index = -1
ret = self._check_target_poses(self.joint_limits)
if ret:
rospy.loginfo("Set current pose as home")
return True
else:
rospy.logerr("Can't calibrate from this position!")
self.start_pose = None
self.target_poses = None
return False
def select_target_pose(self, i):
number_of_target_poses = len(self.target_poses)
if 0 <= i < number_of_target_poses:
rospy.loginfo("Selected pose {} for next movement".format(i))
self.current_pose_index = i
return True
else:
rospy.logerr(
"Index {} is out of bounds: there are {} target poses".format(
i, number_of_target_poses))
return False
def plan_to_start_pose(self):
# TODO: use joint position http://docs.ros.org/melodic/api/moveit_tutorials/html/doc/move_group_python_interface/move_group_python_interface_tutorial.html#planning-to-a-joint-goal
rospy.loginfo("Planning to home pose")
return self._plan_to_pose(self.start_pose)
def plan_to_current_target_pose(self):
# TODO: use joint position http://docs.ros.org/melodic/api/moveit_tutorials/html/doc/move_group_python_interface/move_group_python_interface_tutorial.html#planning-to-a-joint-goal
i = self.current_pose_index
rospy.loginfo("Planning to target pose {}".format(i))
return self._plan_to_pose(self.target_poses[i])
def execute_plan(self):
if self.plan is None:
rospy.logerr("No plan found!")
return False
if CalibrationMovements._is_crazy_plan(self.plan,
self.fallback_joint_limits):
rospy.logerr("Crazy plan found, not executing!")
return False
self.mgc.execute(self.plan)
return True
def _plan_to_pose(self, pose):
self.mgc.set_start_state_to_current_state()
self.mgc.set_pose_target(pose)
ret = self.mgc.plan()
if type(ret) is tuple:
# noetic
success, plan, planning_time, error_code = ret
else:
# melodic
plan = ret
if CalibrationMovements._is_crazy_plan(plan,
self.fallback_joint_limits):
rospy.logwarn("Planning failed")
self.plan = None
return False
else:
rospy.loginfo("Planning successful")
self.plan = plan
return True
def _check_target_poses(self, joint_limits):
if len(self.fallback_joint_limits) == 6:
joint_limits = joint_limits[1:]
for fp in self.target_poses:
self.mgc.set_pose_target(fp)
ret = self.mgc.plan()
if type(ret) is tuple:
# noetic
success, plan, planning_time, error_code = ret
else:
# melodic
plan = ret
if len(plan.joint_trajectory.points
) == 0 or CalibrationMovements._is_crazy_plan(
plan, joint_limits):
return False
return True
@staticmethod
def _compute_poses_around_state(start_pose, angle_delta,
translation_delta):
basis = np.eye(3)
pos_deltas = [
quaternion_from_euler(*rot_axis * angle_delta)
for rot_axis in basis
]
neg_deltas = [
quaternion_from_euler(*rot_axis * (-angle_delta))
for rot_axis in basis
]
quaternion_deltas = list(
chain.from_iterable(zip(pos_deltas, neg_deltas))) # interleave
final_rots = []
for qd in quaternion_deltas:
final_rots.append(list(qd))
# TODO: accept a list of delta values
pos_deltas = [
quaternion_from_euler(*rot_axis * angle_delta / 2)
for rot_axis in basis
]
neg_deltas = [
quaternion_from_euler(*rot_axis * (-angle_delta / 2))
for rot_axis in basis
]
quaternion_deltas = list(
chain.from_iterable(zip(pos_deltas, neg_deltas))) # interleave
for qd in quaternion_deltas:
final_rots.append(list(qd))
final_poses = []
for rot in final_rots:
fp = deepcopy(start_pose)
ori = fp.pose.orientation
combined_rot = quaternion_multiply([ori.x, ori.y, ori.z, ori.w],
rot)
fp.pose.orientation = Quaternion(*combined_rot)
final_poses.append(fp)
fp = deepcopy(start_pose)
fp.pose.position.x += translation_delta / 2
final_poses.append(fp)
fp = deepcopy(start_pose)
fp.pose.position.x -= translation_delta / 2
final_poses.append(fp)
fp = deepcopy(start_pose)
fp.pose.position.y += translation_delta
final_poses.append(fp)
fp = deepcopy(start_pose)
fp.pose.position.y -= translation_delta
final_poses.append(fp)
fp = deepcopy(start_pose)
fp.pose.position.z += translation_delta / 3
final_poses.append(fp)
return final_poses
@staticmethod
def _rot_per_joint(plan, degrees=False):
np_traj = np.array([p.positions for p in plan.joint_trajectory.points])
if len(np_traj) == 0:
raise ValueError
np_traj_max_per_joint = np_traj.max(axis=0)
np_traj_min_per_joint = np_traj.min(axis=0)
ret = abs(np_traj_max_per_joint - np_traj_min_per_joint)
if degrees:
ret = [math.degrees(j) for j in ret]
return ret
@staticmethod
def _is_crazy_plan(plan, max_rotation_per_joint):
abs_rot_per_joint = CalibrationMovements._rot_per_joint(plan)
if (abs_rot_per_joint > max_rotation_per_joint).any():
return True
else:
return False
rospy.Duration(1.0))
# Instantiate a robotcommander object
robot = moveit_commander.RobotCommander()
# Connect to the manipulator move group and get end effector link
manipulator = MoveGroupCommander('manipulator')
end_effector_link = manipulator.get_end_effector_link()
# Set the robot reference frame
manipulator.set_pose_reference_frame('rb07_stand_link')
# Allow replanning to increase the odds of a solution
manipulator.allow_replanning(True)
# Set accuracy in position(meters) and orientation (radians)
manipulator.set_goal_position_tolerance(0.00001)
manipulator.set_goal_orientation_tolerance(0.0001)
manipulator.set_planner_id("RRTkConfigDefault")
tr = tool_tf.transform.translation
print([tr.x, tr.y, tr.z])
# Get a pose to modify
start_pose = manipulator.get_current_pose(end_effector_link).pose
# go home
manipulator.set_named_target('all-zeros')
manipulator.go()
rospy.sleep(1)
# go to vise
go_to_pose('vise', manipulator, start_pose)
# go to g54
go_to_pose('G54', manipulator, start_pose)
class PlannerAnnotationParser(AnnotationParserBase):
"""
Parses the annotations files that contains the benchmarking
information.
"""
def __init__(self, path_to_annotation, path_to_data):
super(PlannerAnnotationParser, self).__init__(path_to_annotation,
path_to_data)
self.parse()
self._load_scene()
self._init_planning()
self.benchmark()
def check_results(self, results):
"""
Returns the results from the planner, checking them against any eventual validation data
(no validation data in our case).
"""
return self.planner_data
def _load_scene(self):
"""
Loads the proper scene for the planner.
It can be either a python static scene or bag containing an occupancy map.
"""
scene = self._annotations["scene"]
for element in scene:
if element["type"] == "launch":
self.play_launch(element["name"])
elif element["type"] == "python":
self.load_python(element["name"])
elif element["type"] == "bag":
self.play_bag(element["name"])
for _ in range(150):
rospy.sleep(0.3)
# wait for the scene to be spawned properly
rospy.sleep(0.5)
def _init_planning(self):
"""
Initialises the needed connections for the planning.
"""
self.group_id = self._annotations["group_id"]
self.planners = self._annotations["planners"]
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
self.group = MoveGroupCommander(self.group_id)
self._marker_pub = rospy.Publisher('/visualization_marker_array',
MarkerArray,
queue_size=10,
latch=True)
self._planning_time_sub = rospy.Subscriber(
'/move_group/result', MoveGroupActionResult,
self._check_computation_time)
rospy.sleep(1)
self.group.set_num_planning_attempts(
self._annotations["planning_attempts"])
self.group.set_goal_tolerance(self._annotations["goal_tolerance"])
self.group.set_planning_time(self._annotations["planning_time"])
self.group.allow_replanning(self._annotations["allow_replanning"])
self._comp_time = []
self.planner_data = []
def benchmark(self):
for test_id, test in enumerate(self._annotations["tests"]):
marker_position_1 = test["start_xyz"]
marker_position_2 = test["goal_xyz"]
self._add_markers(marker_position_1, "Start test \n sequence",
marker_position_2, "Goal")
# Start planning in a given joint position
joints = test["start_joints"]
current = RobotState()
current.joint_state.name = self.robot.get_current_state(
).joint_state.name
current_joints = list(
self.robot.get_current_state().joint_state.position)
current_joints[0:6] = joints
current.joint_state.position = current_joints
self.group.set_start_state(current)
joints = test["goal_joints"]
for planner in self.planners:
if planner == "stomp":
planner = "STOMP"
elif planner == "sbpl":
planner = "AnytimeD*"
self.planner_id = planner
self.group.set_planner_id(planner)
self._plan_joints(
joints,
self._annotations["name"] + "-test_" + str(test_id))
return self.planner_data
def _add_markers(self, point, text1, point_2, text2):
# add marker for start and goal pose of EE
marker_array = MarkerArray()
marker_1 = Marker()
marker_1.header.frame_id = "world"
marker_1.header.stamp = rospy.Time.now()
marker_1.type = Marker.SPHERE
marker_1.scale.x = 0.04
marker_1.scale.y = 0.04
marker_1.scale.z = 0.04
marker_1.lifetime = rospy.Duration()
marker_2 = deepcopy(marker_1)
marker_1.color.g = 0.5
marker_1.color.a = 1.0
marker_1.id = 0
marker_1.pose.position.x = point[0]
marker_1.pose.position.y = point[1]
marker_1.pose.position.z = point[2]
marker_2.color.r = 0.5
marker_2.color.a = 1.0
marker_2.id = 1
marker_2.pose.position.x = point_2[0]
marker_2.pose.position.y = point_2[1]
marker_2.pose.position.z = point_2[2]
marker_3 = Marker()
marker_3.header.frame_id = "world"
marker_3.header.stamp = rospy.Time.now()
marker_3.type = Marker.TEXT_VIEW_FACING
marker_3.scale.z = 0.10
marker_3.lifetime = rospy.Duration()
marker_4 = deepcopy(marker_3)
marker_3.text = text1
marker_3.id = 2
marker_3.color.g = 0.5
marker_3.color.a = 1.0
marker_3.pose.position.x = point[0]
marker_3.pose.position.y = point[1]
marker_3.pose.position.z = point[2] + 0.15
marker_4.text = text2
marker_4.id = 3
marker_4.color.r = 0.5
marker_4.color.a = 1.0
marker_4.pose.position.x = point_2[0]
marker_4.pose.position.y = point_2[1]
marker_4.pose.position.z = point_2[2] + 0.15
marker_array.markers = [marker_1, marker_2, marker_3, marker_4]
self._marker_pub.publish(marker_array)
rospy.sleep(1)
def _plan_joints(self, joints, test_name):
# plan to joint target and determine success
self.group.clear_pose_targets()
group_variable_values = self.group.get_current_joint_values()
group_variable_values[0:6] = joints[0:6]
self.group.set_joint_value_target(group_variable_values)
plan = self.group.plan()
plan_time = "N/A"
total_joint_rotation = "N/A"
comp_time = "N/A"
plan_success = self._check_plan_success(plan)
if plan_success:
plan_time = self._check_plan_time(plan)
total_joint_rotation = self._check_plan_total_rotation(plan)
while not self._comp_time:
rospy.sleep(0.5)
comp_time = self._comp_time.pop(0)
self.planner_data.append([
self.planner_id, test_name,
str(plan_success), plan_time, total_joint_rotation, comp_time
])
@staticmethod
def _check_plan_success(plan):
if len(plan.joint_trajectory.points) > 0:
return True
else:
return False
@staticmethod
def _check_plan_time(plan):
# find duration of successful plan
number_of_points = len(plan.joint_trajectory.points)
time = plan.joint_trajectory.points[number_of_points -
1].time_from_start.to_sec()
return time
@staticmethod
def _check_plan_total_rotation(plan):
# find total joint rotation in successful trajectory
angles = [0, 0, 0, 0, 0, 0]
number_of_points = len(plan.joint_trajectory.points)
for i in range(number_of_points - 1):
angles_temp = [
abs(x - y)
for x, y in zip(plan.joint_trajectory.points[i + 1].positions,
plan.joint_trajectory.points[i].positions)
]
angles = [x + y for x, y in zip(angles, angles_temp)]
total_angle_change = sum(angles)
return total_angle_change
def _check_computation_time(self, msg):
# get computation time for successful plan to be found
if msg.status.status == 3:
self._comp_time.append(msg.result.planning_time)
def _check_path_length(self, plan):
# find distance travelled by end effector
# not yet implemented
return
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Construct the initial scene object
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# Create a dictionary to hold object colors
self.colors = dict()
# Pause for the scene to get ready
rospy.sleep(1)
# Initialize the move group for the right arm
right_arm = MoveGroupCommander('right_arm')
# Initialize the move group for the left arm
left_arm = MoveGroupCommander('left_arm')
right_arm.set_planner_id("KPIECEkConfigDefault");
left_arm.set_planner_id("KPIECEkConfigDefault");
rospy.sleep(1)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.01)
right_arm.set_goal_orientation_tolerance(0.05)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the reference frame for pose targets
reference_frame = 'base_footprint'
# Set the right arm reference frame accordingly
right_arm.set_pose_reference_frame(reference_frame)
# Allow 5 seconds per planning attempt
right_arm.set_planning_time(5)
# Give each of the scene objects a unique name
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
#left_arm.set_named_target('left_start')
#left_arm.go()
# Start the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_start')
right_arm.go()
rospy.sleep(2)
# Set the height of the table off the ground
table_ground = 0.75
# Set the length, width and height of the table and boxes
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# Add a table top and two boxes to the scene
table_pose = PoseStamped()
table_pose.header.frame_id = reference_frame
table_pose.pose.position.x = 0.56
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = reference_frame
box1_pose.pose.position.x = 0.51
box1_pose.pose.position.y = -0.1
box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = reference_frame
box2_pose.pose.position.x = 0.49
box2_pose.pose.position.y = 0.15
box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
# Make the table red and the boxes orange
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the target pose in between the boxes and above the table
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.pose.position.x = 0.4
target_pose.pose.position.y = 0.0
target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05
target_pose.pose.orientation.w = 1.0
# Set the target pose for the arm
right_arm.set_pose_target(target_pose, end_effector_link)
# Move the arm to the target pose (if possible)
right_arm.go()
# Pause for a moment...
rospy.sleep(2)
# Return the arm to the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_start')
right_arm.go()
# Exit MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
class SrRobotCommander(object):
"""
Base class for hand and arm commanders
"""
def __init__(self, name):
"""
Initialize MoveGroupCommander object
@param name - name of the MoveIt group
"""
self._name = name
self._move_group_commander = MoveGroupCommander(name)
self._robot_commander = RobotCommander()
self._robot_name = self._robot_commander._r.get_robot_name()
self.refresh_named_targets()
self._warehouse_name_get_srv = rospy.ServiceProxy(
"get_robot_state", GetState)
self._planning_scene = PlanningSceneInterface()
self._joint_states_lock = threading.Lock()
self._joint_states_listener = \
rospy.Subscriber("joint_states", JointState,
self._joint_states_callback, queue_size=1)
self._joints_position = {}
self._joints_velocity = {}
self._joints_effort = {}
self._joints_state = None
self._clients = {}
self.__plan = None
self._controllers = {}
rospy.wait_for_service('compute_ik')
self._compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
self._forward_k = rospy.ServiceProxy('compute_fk', GetPositionFK)
controller_list_param = rospy.get_param("/move_group/controller_list")
# create dictionary with name of controllers and corresponding joints
self._controllers = {
item["name"]: item["joints"]
for item in controller_list_param
}
self._set_up_action_client(self._controllers)
self.tf_buffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tf_buffer)
threading.Thread(None, rospy.spin)
def set_planner_id(self, planner_id):
self._move_group_commander.set_planner_id(planner_id)
def set_num_planning_attempts(self, num_planning_attempts):
self._move_group_commander.set_num_planning_attempts(
num_planning_attempts)
def set_planning_time(self, seconds):
self._move_group_commander.set_planning_time(seconds)
def get_end_effector_pose_from_named_state(self, name):
state = self._warehouse_name_get_srv(name, self._robot_name).state
return self.get_end_effector_pose_from_state(state)
def get_end_effector_pose_from_state(self, state):
header = Header()
fk_link_names = [self._move_group_commander.get_end_effector_link()]
header.frame_id = self._move_group_commander.get_pose_reference_frame()
response = self._forward_k(header, fk_link_names, state)
return response.pose_stamped[0]
def get_planning_frame(self):
return self._move_group_commander.get_planning_frame()
def set_pose_reference_frame(self, reference_frame):
self._move_group_commander.set_pose_reference_frame(reference_frame)
def get_group_name(self):
return self._name
def refresh_named_targets(self):
self._srdf_names = self.__get_srdf_names()
self._warehouse_names = self.__get_warehouse_names()
def set_max_velocity_scaling_factor(self, value):
self._move_group_commander.set_max_velocity_scaling_factor(value)
def set_max_acceleration_scaling_factor(self, value):
self._move_group_commander.set_max_acceleration_scaling_factor(value)
def allow_looking(self, value):
self._move_group_commander.allow_looking(value)
def allow_replanning(self, value):
self._move_group_commander.allow_replanning(value)
def execute(self):
"""
Executes the last plan made.
"""
if self.check_plan_is_valid():
self._move_group_commander.execute(self.__plan)
self.__plan = None
else:
rospy.logwarn("No plans were made, not executing anything.")
def execute_plan(self, plan):
if self.check_given_plan_is_valid(plan):
self._move_group_commander.execute(plan)
self.__plan = None
else:
rospy.logwarn("Plan is not valid, not executing anything.")
def move_to_joint_value_target(self,
joint_states,
wait=True,
angle_degrees=False):
"""
Set target of the robot's links and moves to it.
@param joint_states - dictionary with joint name and value. It can
contain only joints values of which need to be changed.
@param wait - should method wait for movement end or not
@param angle_degrees - are joint_states in degrees or not
"""
joint_states_cpy = copy.deepcopy(joint_states)
if angle_degrees:
joint_states_cpy.update(
(joint, radians(i)) for joint, i in joint_states_cpy.items())
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_joint_value_target(joint_states_cpy)
self._move_group_commander.go(wait=wait)
def plan_to_joint_value_target(self, joint_states, angle_degrees=False):
"""
Set target of the robot's links and plans.
@param joint_states - dictionary with joint name and value. It can
contain only joints values of which need to be changed.
@param angle_degrees - are joint_states in degrees or not
This is a blocking method.
"""
joint_states_cpy = copy.deepcopy(joint_states)
if angle_degrees:
joint_states_cpy.update(
(joint, radians(i)) for joint, i in joint_states_cpy.items())
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_joint_value_target(joint_states_cpy)
self.__plan = self._move_group_commander.plan()
return self.__plan
def check_plan_is_valid(self):
"""
Checks if current plan contains a valid trajectory
"""
return (self.__plan is not None
and len(self.__plan.joint_trajectory.points) > 0)
def check_given_plan_is_valid(self, plan):
"""
Checks if given plan contains a valid trajectory
"""
return (plan is not None and len(plan.joint_trajectory.points) > 0)
def get_robot_name(self):
return self._robot_name
def named_target_in_srdf(self, name):
return name in self._srdf_names
def set_named_target(self, name):
if name in self._srdf_names:
self._move_group_commander.set_named_target(name)
elif (name in self._warehouse_names):
response = self._warehouse_name_get_srv(name, self._robot_name)
active_names = self._move_group_commander._g.get_active_joints()
joints = response.state.joint_state.name
positions = response.state.joint_state.position
js = {}
for n, this_name in enumerate(joints):
if this_name in active_names:
js[this_name] = positions[n]
self._move_group_commander.set_joint_value_target(js)
else:
rospy.logerr("Unknown named state '%s'..." % name)
return False
return True
def get_named_target_joint_values(self, name):
output = dict()
if (name in self._srdf_names):
output = self._move_group_commander.\
_g.get_named_target_values(str(name))
elif (name in self._warehouse_names):
js = self._warehouse_name_get_srv(
name, self._robot_name).state.joint_state
for x, n in enumerate(js.name):
if n in self._move_group_commander._g.get_joints():
output[n] = js.position[x]
else:
rospy.logerr("No target named %s" % name)
return None
return output
def get_end_effector_link(self):
return self._move_group_commander.get_end_effector_link()
def get_current_pose(self, reference_frame=None):
"""
Get the current pose of the end effector.
@param reference_frame - The desired reference frame in which end effector pose should be returned.
If none is passed, it will use the planning frame as reference.
@return geometry_msgs.msg.Pose() - current pose of the end effector
"""
if reference_frame is not None:
try:
trans = self.tf_buffer.lookup_transform(
reference_frame,
self._move_group_commander.get_end_effector_link(),
rospy.Time(0), rospy.Duration(5.0))
current_pose = geometry_msgs.msg.Pose()
current_pose.position.x = trans.transform.translation.x
current_pose.position.y = trans.transform.translation.y
current_pose.position.z = trans.transform.translation.z
current_pose.orientation.x = trans.transform.rotation.x
current_pose.orientation.y = trans.transform.rotation.y
current_pose.orientation.z = trans.transform.rotation.z
current_pose.orientation.w = trans.transform.rotation.w
return current_pose
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
rospy.logwarn(
"Couldn't get the pose from " +
self._move_group_commander.get_end_effector_link() +
" in " + reference_frame + " reference frame")
return None
else:
return self._move_group_commander.get_current_pose().pose
def get_current_state(self):
"""
Get the current joint state of the group being used.
@return a dictionary with the joint names as keys and current joint values
"""
joint_names = self._move_group_commander._g.get_active_joints()
joint_values = self._move_group_commander._g.get_current_joint_values()
return dict(zip(joint_names, joint_values))
def get_current_state_bounded(self):
"""
Get the current joint state of the group being used, enforcing that they are within each joint limits.
@return a dictionary with the joint names as keys and current joint values
"""
current = self._move_group_commander._g.get_current_state_bounded()
names = self._move_group_commander._g.get_active_joints()
output = {n: current[n] for n in names if n in current}
return output
def get_robot_state_bounded(self):
return self._move_group_commander._g.get_current_state_bounded()
def move_to_named_target(self, name, wait=True):
"""
Set target of the robot's links and moves to it
@param name - name of the target pose defined in SRDF
@param wait - should method wait for movement end or not
"""
self._move_group_commander.set_start_state_to_current_state()
if self.set_named_target(name):
self._move_group_commander.go(wait=wait)
def plan_to_named_target(self, name):
"""
Set target of the robot's links and plans
This is a blocking method.
@param name - name of the target pose defined in SRDF
"""
self._move_group_commander.set_start_state_to_current_state()
if self.set_named_target(name):
self.__plan = self._move_group_commander.plan()
def __get_warehouse_names(self):
try:
list_srv = rospy.ServiceProxy("list_robot_states", ListStates)
return list_srv("", self._robot_name).states
except rospy.ServiceException as exc:
rospy.logwarn("Couldn't access warehouse: " + str(exc))
return list()
def _reset_plan(self):
self.__plan = None
def _set_plan(self, plan):
self.__plan = plan
def __get_srdf_names(self):
return self._move_group_commander._g.get_named_targets()
def get_named_targets(self):
"""
Get the complete list of named targets, from SRDF
as well as warehouse poses if available.
@return list of strings containing names of targets.
"""
return self._srdf_names + self._warehouse_names
def get_joints_position(self):
"""
Returns joints position
@return - dictionary with joints positions
"""
with self._joint_states_lock:
return self._joints_position
def get_joints_velocity(self):
"""
Returns joints velocities
@return - dictionary with joints velocities
"""
with self._joint_states_lock:
return self._joints_velocity
def _get_joints_effort(self):
"""
Returns joints effort
@return - dictionary with joints efforts
"""
with self._joint_states_lock:
return self._joints_effort
def get_joints_state(self):
"""
Returns joints state
@return - JointState message
"""
with self._joint_states_lock:
return self._joints_state
def run_joint_trajectory(self, joint_trajectory):
"""
Moves robot through all joint states with specified timeouts
@param joint_trajectory - JointTrajectory class object. Represents
trajectory of the joints which would be executed.
"""
plan = RobotTrajectory()
plan.joint_trajectory = joint_trajectory
self._move_group_commander.execute(plan)
def make_named_trajectory(self, trajectory):
"""
Makes joint value trajectory from specified by named poses (either from
SRDF or from warehouse)
@param trajectory - list of waypoints, each waypoint is a dict with
the following elements (n.b either name or joint_angles is required)
- name -> the name of the way point
- joint_angles -> a dict of joint names and angles
- interpolate_time -> time to move from last wp
- pause_time -> time to wait at this wp
- degrees -> set to true if joint_angles is specified in degrees. Assumed false if absent.
"""
current = self.get_current_state_bounded()
joint_trajectory = JointTrajectory()
joint_names = current.keys()
joint_trajectory.joint_names = joint_names
start = JointTrajectoryPoint()
start.positions = current.values()
start.time_from_start = rospy.Duration.from_sec(0.001)
joint_trajectory.points.append(start)
time_from_start = 0.0
for wp in trajectory:
joint_positions = None
if 'name' in wp.keys():
joint_positions = self.get_named_target_joint_values(
wp['name'])
elif 'joint_angles' in wp.keys():
joint_positions = copy.deepcopy(wp['joint_angles'])
if 'degrees' in wp.keys() and wp['degrees']:
for joint, angle in joint_positions.iteritems():
joint_positions[joint] = radians(angle)
if joint_positions is None:
rospy.logerr(
"Invalid waypoint. Must contain valid name for named target or dict of joint angles."
)
return None
new_positions = {}
for n in joint_names:
new_positions[n] = joint_positions[
n] if n in joint_positions else current[n]
trajectory_point = JointTrajectoryPoint()
trajectory_point.positions = [
new_positions[n] for n in joint_names
]
current = new_positions
time_from_start += wp['interpolate_time']
trajectory_point.time_from_start = rospy.Duration.from_sec(
time_from_start)
joint_trajectory.points.append(trajectory_point)
if 'pause_time' in wp and wp['pause_time'] > 0:
extra = JointTrajectoryPoint()
extra.positions = trajectory_point.positions
time_from_start += wp['pause_time']
extra.time_from_start = rospy.Duration.from_sec(
time_from_start)
joint_trajectory.points.append(extra)
return joint_trajectory
def send_stop_trajectory_unsafe(self):
"""
Sends a trajectory of all active joints at their current position.
This stops the robot.
"""
current = self.get_current_state_bounded()
trajectory_point = JointTrajectoryPoint()
trajectory_point.positions = current.values()
trajectory_point.time_from_start = rospy.Duration.from_sec(0.1)
trajectory = JointTrajectory()
trajectory.points.append(trajectory_point)
trajectory.joint_names = current.keys()
self.run_joint_trajectory_unsafe(trajectory)
def run_named_trajectory_unsafe(self, trajectory, wait=False):
"""
Moves robot through trajectory specified by named poses, either from
SRDF or from warehouse. Runs trajectory directly via contoller.
@param trajectory - list of waypoints, each waypoint is a dict with
the following elements:
- name -> the name of the way point
- interpolate_time -> time to move from last wp
- pause_time -> time to wait at this wp
"""
joint_trajectory = self.make_named_trajectory(trajectory)
if joint_trajectory is not None:
self.run_joint_trajectory_unsafe(joint_trajectory, wait)
def run_named_trajectory(self, trajectory):
"""
Moves robot through trajectory specified by named poses, either from
SRDF or from warehouse. Runs trajectory via moveit.
@param trajectory - list of waypoints, each waypoint is a dict with
the following elements:
- name -> the name of the way point
- interpolate_time -> time to move from last wp
- pause_time -> time to wait at this wp
"""
joint_trajectory = self.make_named_trajectory(trajectory)
if joint_trajectory is not None:
self.run_joint_trajectory(joint_trajectory)
def move_to_position_target(self, xyz, end_effector_link="", wait=True):
"""
Specify a target position for the end-effector and moves to it
@param xyz - new position of end-effector
@param end_effector_link - name of the end effector link
@param wait - should method wait for movement end or not
"""
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_position_target(xyz, end_effector_link)
self._move_group_commander.go(wait=wait)
def plan_to_position_target(self, xyz, end_effector_link=""):
"""
Specify a target position for the end-effector and plans.
This is a blocking method.
@param xyz - new position of end-effector
@param end_effector_link - name of the end effector link
"""
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_position_target(xyz, end_effector_link)
self.__plan = self._move_group_commander.plan()
def move_to_pose_target(self, pose, end_effector_link="", wait=True):
"""
Specify a target pose for the end-effector and moves to it
@param pose - new pose of end-effector: a Pose message, a PoseStamped
message or a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list
of 7 floats [x, y, z, qx, qy, qz, qw]
@param end_effector_link - name of the end effector link
@param wait - should method wait for movement end or not
"""
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_pose_target(pose, end_effector_link)
self._move_group_commander.go(wait=wait)
def plan_to_pose_target(self,
pose,
end_effector_link="",
alternative_method=False):
"""
Specify a target pose for the end-effector and plans.
This is a blocking method.
@param pose - new pose of end-effector: a Pose message, a PoseStamped
message or a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list
of 7 floats [x, y, z, qx, qy, qz, qw]
@param end_effector_link - name of the end effector link
@param alternative_method - use set_joint_value_target instead of set_pose_target
"""
self._move_group_commander.set_start_state_to_current_state()
if alternative_method:
self._move_group_commander.set_joint_value_target(
pose, end_effector_link)
else:
self._move_group_commander.set_pose_target(pose, end_effector_link)
self.__plan = self._move_group_commander.plan()
return self.__plan
def _joint_states_callback(self, joint_state):
"""
The callback function for the topic joint_states.
It will store the received joint position, velocity and efforts
information into dictionaries
@param joint_state - the message containing the joints data.
"""
with self._joint_states_lock:
self._joints_state = joint_state
self._joints_position = {
n: p
for n, p in zip(joint_state.name, joint_state.position)
}
self._joints_velocity = {
n: v
for n, v in zip(joint_state.name, joint_state.velocity)
}
self._joints_effort = {
n: v
for n, v in zip(joint_state.name, joint_state.effort)
}
def _set_up_action_client(self, controller_list):
"""
Sets up an action client to communicate with the trajectory controller
"""
self._action_running = {}
for controller_name in controller_list.keys():
self._action_running[controller_name] = False
service_name = controller_name + "/follow_joint_trajectory"
self._clients[controller_name] = SimpleActionClient(
service_name, FollowJointTrajectoryAction)
if self._clients[controller_name].wait_for_server(
timeout=rospy.Duration(4)) is False:
err_msg = 'Failed to connect to action server ({}) in 4 sec'.format(
service_name)
rospy.logwarn(err_msg)
def move_to_joint_value_target_unsafe(self,
joint_states,
time=0.002,
wait=True,
angle_degrees=False):
"""
Set target of the robot's links and moves to it.
@param joint_states - dictionary with joint name and value. It can
contain only joints values of which need to be changed.
@param time - time in s (counting from now) for the robot to reach the
target (it needs to be greater than 0.0 for it not to be rejected by
the trajectory controller)
@param wait - should method wait for movement end or not
@param angle_degrees - are joint_states in degrees or not
"""
# self._update_default_trajectory()
# self._set_targets_to_default_trajectory(joint_states)
goals = {}
joint_states_cpy = copy.deepcopy(joint_states)
if angle_degrees:
joint_states_cpy.update(
(joint, radians(i)) for joint, i in joint_states_cpy.items())
for controller in self._controllers:
controller_joints = self._controllers[controller]
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = []
point = JointTrajectoryPoint()
point.positions = []
for x in joint_states_cpy.keys():
if x in controller_joints:
goal.trajectory.joint_names.append(x)
point.positions.append(joint_states_cpy[x])
point.time_from_start = rospy.Duration.from_sec(time)
goal.trajectory.points = [point]
goals[controller] = goal
self._call_action(goals)
if not wait:
return
for i in self._clients.keys():
if not self._clients[i].wait_for_result():
rospy.loginfo("Trajectory not completed")
def action_is_running(self, controller=None):
if controller is not None:
return self._action_running[controller]
for controller_running in self._action_running.values():
if controller_running:
return True
return False
def _action_done_cb(self, controller, terminal_state, result):
self._action_running[controller] = False
def _call_action(self, goals):
for client in self._clients:
self._action_running[client] = True
self._clients[client].send_goal(
goals[client],
lambda terminal_state, result: self._action_done_cb(
client, terminal_state, result))
def run_joint_trajectory_unsafe(self, joint_trajectory, wait=True):
"""
Moves robot through all joint states with specified timeouts
@param joint_trajectory - JointTrajectory class object. Represents
trajectory of the joints which would be executed.
@param wait - should method wait for movement end or not
"""
goals = {}
for controller in self._controllers:
controller_joints = self._controllers[controller]
goal = FollowJointTrajectoryGoal()
goal.trajectory = copy.deepcopy(joint_trajectory)
indices_of_joints_in_this_controller = []
for i, joint in enumerate(joint_trajectory.joint_names):
if joint in controller_joints:
indices_of_joints_in_this_controller.append(i)
goal.trajectory.joint_names = [
joint_trajectory.joint_names[i]
for i in indices_of_joints_in_this_controller
]
for point in goal.trajectory.points:
if point.positions:
point.positions = [
point.positions[i]
for i in indices_of_joints_in_this_controller
]
if point.velocities:
point.velocities = [
point.velocities[i]
for i in indices_of_joints_in_this_controller
]
if point.effort:
point.effort = [
point.effort[i]
for i in indices_of_joints_in_this_controller
]
goals[controller] = goal
self._call_action(goals)
if not wait:
return
for i in self._clients.keys():
if not self._clients[i].wait_for_result():
rospy.loginfo("Trajectory not completed")
def plan_to_waypoints_target(self,
waypoints,
reference_frame=None,
eef_step=0.005,
jump_threshold=0.0):
"""
Specify a set of waypoints for the end-effector and plans.
This is a blocking method.
@param reference_frame - the reference frame in which the waypoints are given
@param waypoints - an array of poses of end-effector
@param eef_step - configurations are computed for every eef_step meters
@param jump_threshold - maximum distance in configuration space between consecutive points in the resulting path
"""
old_frame = self._move_group_commander.get_pose_reference_frame()
if reference_frame is not None:
self.set_pose_reference_frame(reference_frame)
(self.__plan,
fraction) = self._move_group_commander.compute_cartesian_path(
waypoints, eef_step, jump_threshold)
self.set_pose_reference_frame(old_frame)
def set_teach_mode(self, teach):
"""
Activates/deactivates the teach mode for the robot.
Activation: stops the the trajectory controllers for the robot, and
sets it to teach mode.
Deactivation: stops the teach mode and starts trajectory controllers
for the robot.
Currently this method blocks for a few seconds when called on a hand,
while the hand parameters are reloaded.
@param teach - bool to activate or deactivate teach mode
"""
if teach:
mode = RobotTeachModeRequest.TEACH_MODE
else:
mode = RobotTeachModeRequest.TRAJECTORY_MODE
self.change_teach_mode(mode, self._name)
def move_to_trajectory_start(self, trajectory, wait=True):
"""
Make and execute a plan from the current state to the first state in an pre-existing trajectory
@param trajectory - moveit_msgs/JointTrajectory
@param wait - Bool to specify if movement should block untill finished.
"""
if len(trajectory.points) <= 0:
rospy.logerr("Trajectory has no points in it, can't reverse...")
return None
first_point = trajectory.points[0]
end_state = dict(zip(trajectory.joint_names, first_point.positions))
self.move_to_joint_value_target(end_state, wait=wait)
@staticmethod
def change_teach_mode(mode, robot):
teach_mode_client = rospy.ServiceProxy('/teach_mode', RobotTeachMode)
req = RobotTeachModeRequest()
req.teach_mode = mode
req.robot = robot
try:
resp = teach_mode_client(req)
if resp.result == RobotTeachModeResponse.ERROR:
rospy.logerr("Failed to change robot %s to mode %d", robot,
mode)
else:
rospy.loginfo("Changed robot %s to mode %d Result = %d", robot,
mode, resp.result)
except rospy.ServiceException:
rospy.logerr("Failed to call service teach_mode")
def get_ik(self, target_pose, avoid_collisions=False, joint_states=None):
"""
Computes the inverse kinematics for a given pose. It returns a JointState
@param target_pose - A given pose of type PoseStamped
@param avoid_collisions - Find an IK solution that avoids collisions. By default, this is false
"""
service_request = PositionIKRequest()
service_request.group_name = self._name
service_request.ik_link_name = self._move_group_commander.get_end_effector_link(
)
service_request.pose_stamped = target_pose
service_request.timeout.secs = 0.5
service_request.avoid_collisions = avoid_collisions
if joint_states is None:
service_request.robot_state.joint_state = self.get_joints_state()
else:
service_request.robot_state.joint_state = joint_states
try:
resp = self._compute_ik(ik_request=service_request)
# Check if error_code.val is SUCCESS=1
if resp.error_code.val != 1:
if resp.error_code.val == -10:
rospy.logerr("Unreachable point: Start state in collision")
elif resp.error_code.val == -12:
rospy.logerr("Unreachable point: Goal state in collision")
elif resp.error_code.val == -31:
rospy.logerr("Unreachable point: No IK solution")
else:
rospy.logerr("Unreachable point (error: %s)" %
resp.error_code)
return
else:
return resp.solution.joint_state
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s" % e)
class Arm_Controller:
def __init__(self):
# Give the launch a chance to catch up
# rospy.sleep(5)
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('Arm_Controller')
rospy.loginfo("Launched Arm Controller")
# constants
self.GROUP_NAME_ARM = 'arm'
self.GRIPPER_FRAME = 'gripper_link'
self.REFERENCE_FRAME = 'base_link'
self.ARM_BASE_FRAME = 'arm_base_link'
self.done = True
self.test_pose_publisher = rospy.Publisher('/test_arm_pose', PoseStamped)
rospy.Subscriber("/arm_target_pose", PoseStamped, self.move_arm_to_pose, queue_size=1)
self.robot_name = "gatlin"
move_arm_service = createService('gatlin/move/arm', MoveRobot, self.handle_move_arm)
# We need a tf listener to convert poses into arm reference base
self.tfl = tf.TransformListener()
# Initialize the move group for the right arm
self.arm = MoveGroupCommander(self.GROUP_NAME_ARM)
self.gripper = Gripper()
self.robot = moveit_commander.RobotCommander()
# Allow replanning to increase the odds of a solution
self.arm.allow_replanning(True)
# Set the planner
self.arm.set_planner_id("RRTConnectkConfigDefault")
# Set the right arm reference frame
self.arm.set_pose_reference_frame(self.REFERENCE_FRAME)
# Give the scene a chance to catch up
rospy.sleep(1)
# Allow some leeway in position (meters) and orientation (radians)
# USELESS; do not work on pick and place! Explained on this issue:
# https://github.com/ros-planning/moveit_ros/issues/577
self.arm.set_goal_position_tolerance(0.005)
self.arm.set_goal_orientation_tolerance(0.05)
# Allow 2 seconds per planning attempt
self.arm.set_planning_time(2.0)
# Create a quaternion from the Euler angles
#q = quaternion_from_euler(0, pitch, yaw)
# horiz = Quaternion(-0.023604, 0.99942, 0.00049317, 0.024555)
# deg45 = Quaternion(-0.022174, 0.9476, 0.0074215, 0.31861)
down = Quaternion(-0.00035087, 0.73273, 0.00030411, 0.68052)
# back_pos = Point(-0.03, 0.0313, 0.476)
# init rest pose
self.rest_pose = PoseStamped()
self.rest_pose.header.frame_id = self.REFERENCE_FRAME
self.rest_pose.pose.position = Point(-0.06, 0.00, 0.35)
self.rest_pose.pose.orientation = Quaternion(0.0251355325061, 0.982948881414, -0.0046583987932, 0.182093384981)
# init place_upper pose
self.place_upper_pose = PoseStamped()
self.place_upper_pose.header.frame_id = self.REFERENCE_FRAME
self.place_upper_pose.pose.position = Point(0.24713, -0.0051618, 0.37998)
self.place_upper_pose.pose.orientation = Quaternion(0.0030109, 0.1472, -0.020231, 0.9889)
# init current pose
self.current_pose = PoseStamped()
self.current_pose.header.frame_id = self.REFERENCE_FRAME
self.current_pose.pose.position = Point(0,0,0)
self.current_pose.pose.orientation = down
# Open the gripper
rospy.loginfo("Set Gripper: open")
self.gripper.set(1.0)
# self.arm.set_pose_target(self.rest_pose)
# self.arm.go()
# rospy.sleep(1)
# self.ar = ArbotixROS()
# rate = rospy.Rate(30)
# while not rospy.is_shutdown():
# # rospy.logerr(self.ar.getVoltage(4))
# # rospy.logerr(self.ar.getSpeed(5))
# rospy.logerr(self.ar.getPosition(1))
# rate.sleep()
rospy.spin()
def MoveToPoseWithIntermediate(self, ps, offsets) :
success = False
for offset in offsets:
# interpose = getOffsetPose(hand_pose, offset)
interpose = getOffsetPose(ps, offset)
success = self.MoveToPose(interpose, "MoveToIntermediatePose")
rospy.sleep(1)
success = self.MoveToPose(ps, "MoveToPose")
return success
def MoveToPose(self, ps, name) :
newpose = self.transform_pose(self.REFERENCE_FRAME, ps)
down = Quaternion(-0.00035087, 0.73273, 0.00030411, 0.68052)
newpose.pose.orientation = down
if self.move_arm_to_pose(newpose) :
rospy.loginfo("SUCCEEDED: %s" % name)
return True
else :
rospy.logerr("FAILED %s" % name)
return False
def move_arm_to_pose(self, ps):
arm_target_pose = deepcopy(ps)
arm_target_pose.header.stamp = rospy.Time.now()
self.test_pose_publisher.publish(arm_target_pose)
self.arm.set_pose_target(arm_target_pose)
success = self.arm.go()
return success
def handle_move_arm(self, req):
success = True
gripper = self.gripper
if req.action == "OPEN_GRIPPER":
rospy.loginfo("Beginning to open gripper")
gripper.open(block=True)
rospy.loginfo("Opened Gripper")
elif req.action == "CLOSE_GRIPPER" :
rospy.loginfo("Beginning to close Gripper")
gripper.close(block=True)
rospy.loginfo("Closed Gripper")
elif req.action == "MOVE_TO_POSE_INTERMEDIATE" :
rospy.loginfo("Trying to Move To Pose With Intermediate")
offsets = [Vector3(0,0,.07)]
success = self.MoveToPoseWithIntermediate(req.ps, offsets)
elif req.action == "MOVE_TO_POSE" :
rospy.loginfo("Trying to Move To Pose")
success = self.MoveToPose(req.ps, "FAILED MoveToPose")
elif req.action == "RESET_ARM" :
rospy.loginfo("Trying to Move To Rest Pose")
success = self.move_arm_to_pose(self.rest_pose)
elif req.action == "PLACE_UPPER" :
rospy.loginfo("Trying to Move To Place Upper Pose")
success = self.move_arm_to_pose(self.place_upper_pose)
# success = self.MoveToPose(self.rest_pose, "FAILED MoveToRestPose")
# elif req.action == "MOVE_TO_POS" :
# rospy.loginfo("Trying to Move To Pos")
# new_pose = Pose()
# if req.limb == 'left':
# try:
# self.initial_left
# new_pose = deepcopy(self.initial_left)
# except AttributeError:
# new_pose = deepcopy(self.hand_pose_left)
# self.initial_left = deepcopy(self.hand_pose_left)
# elif req.limb == 'right':
# try:
# self.initial_right
# new_pose = deepcopy(self.initial_right)
# except AttributeError:
# new_pose = deepcopy(self.hand_pose_right)
# self.initial_right = deepcopy(self.hand_pose_right)
# new_pose.position = deepcopy(req.pose.position)
# # success = self.MoveToPose(req.limb, new_pose, "FAILED MoveToPose")
# success = self.MoveToPoseWithIntermediate(req.limb, new_pose)
# rospy.loginfo("Moved to pos: %r" % success)
else :
success = False
rospy.logerr("invalid action")
return MoveRobotResponse(success)
def orientation_cb(self, data):
if(data.x == -1.0 and data.y == -2.0):
print "################################################"
print "################### Orientation! #############"
print "################################################"
deg = data.z*15
rad = -deg * pi/180 + pi/2
print rad
q = quaternion_from_euler(0,rad,0)
self.current_pose.pose.orientation.x = q[0]
self.current_pose.pose.orientation.y = q[1]
self.current_pose.pose.orientation.z = q[2]
self.current_pose.pose.orientation.w = q[3]
else:
return
# transform the pose stamped to the new frame
def transform_pose(self, new_frame, pose):
if pose.header.frame_id == new_frame:
return pose
try:
ps = deepcopy(pose)
ps.header.stamp = rospy.Time(0)
self.tfl.waitForTransform(ps.header.frame_id, new_frame, rospy.Time(0), rospy.Duration(4.0))
new_pose = self.tfl.transformPose(new_frame, ps)
new_pose.header.stamp = deepcopy(pose.header.stamp)
return new_pose
except Exception as e:
rospy.logerr(e)
rospy.logerr("no transform")
return None
