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 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 execute(self, userdata):
mc = MoveGroupCommander("ext")
mc.set_joint_value_target("ext_axis", userdata.position)
plan = mc.plan()
userdata.trajectory = plan.joint_trajectory
if not plan.joint_trajectory.points:
return 'aborted'
return 'succeeded'
def execute(self, userdata):
goal = userdata.goal
mc = MoveGroupCommander("r" + str(userdata.robot) + "_arm")
for joint in goal.__slots__:
mc.set_joint_value_target("r" + str(userdata.robot) + "_joint_" + str(joint), getattr(goal, joint))
plan = mc.plan()
userdata.trajectory = plan.joint_trajectory
if not plan.joint_trajectory.points:
return 'aborted'
return 'succeeded'
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_speed_demo')
# Initialize the move group for the right arm
arm = MoveGroupCommander('manipulator')
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Allow some leeway in position(meters) and orientation (radians)
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.01)
# Start the arm in the "home" pose stored in the SRDF file
arm.set_named_target('home')
arm.go()
# 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
joint_positions = [0.391410, -0.676384, -0.376217, 0.0, 1.052834, 0.454125]
arm.set_joint_value_target(joint_positions)
# 控制机械臂完成运动
arm.go()
# Start the arm in the "home" pose stored in the SRDF file
arm.set_named_target('home')
arm.go()
# Get back the planned trajectory
arm.set_joint_value_target(joint_positions)
traj = arm.plan()
# Scale the trajectory speed by a factor of 0.25
new_traj = scale_trajectory_speed(traj, 0.25)
# Execute the new trajectory
arm.execute(new_traj)
rospy.sleep(1)
arm.set_named_target('home')
arm.go()
# Exit MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def execute(self, userdata):
goal_r1 = MA1400JointState()
goal_r2 = MA1400JointState()
mc = MoveGroupCommander("arms")
for joint in goal_r1.__slots__:
mc.set_joint_value_target("r1_joint_" + str(joint), getattr(goal_r1, joint))
for joint in goal_r2.__slots__:
mc.set_joint_value_target("r2_joint_" + str(joint), getattr(goal_r2, joint))
plan = mc.plan()
userdata.trajectory = plan.joint_trajectory
if not plan.joint_trajectory.points:
return 'aborted'
return 'succeeded'
def execute(self, userdata):
group = resolve_planning_group(userdata.ik_link)
if not group:
return 'aborted'
(goal, tip_frame) = transform_to_tip(group, userdata.ik_link, userdata.goal)
mc = MoveGroupCommander(group)
mc.set_pose_target(goal, tip_frame)
plan = mc.plan()
userdata.trajectory = plan.joint_trajectory
if not plan.joint_trajectory.points:
return 'aborted'
return 'succeeded'
def execute(self, userdata):
group = 'arms'
mc = MoveGroupCommander(group)
group_1 = resolve_planning_group(userdata.ik_link_1)
(goal_1, tip_frame_1) = transform_to_tip(group_1, userdata.ik_link_1, userdata.goal_1)
group_2 = resolve_planning_group(userdata.ik_link_2)
(goal_2, tip_frame_2) = transform_to_tip(group_2, userdata.ik_link_2, userdata.goal_2)
mc.set_pose_target(goal_1, tip_frame_1)
mc.set_pose_target(goal_2, tip_frame_2)
plan = mc.plan()
userdata.trajectory = plan.joint_trajectory
if not plan.joint_trajectory.points:
return 'aborted'
return 'succeeded'
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Initialize the move group for the right arm
right_arm = MoveGroupCommander('right_arm')
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Allow some leeway in position(meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.02)
right_arm.set_goal_orientation_tolerance(0.1)
# Start the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('rest')
right_arm.go()
# Move to the named "straight_forward" pose stored in the SRDF file
right_arm.set_named_target('wave')
right_arm.go()
rospy.sleep(2)
# Move back to the resting position at roughly 1/4 speed
right_arm.set_named_target('rest')
# Get back the planned trajectory
traj = right_arm.plan()
# Set the trajectory speed
new_traj = create_tracking_trajectory(traj, 1.0)
rospy.loginfo(new_traj)
# Execute the new trajectory
right_arm.execute(new_traj)
rospy.sleep(1)
# Exit MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def talker_by13():
#init
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_fk_demo')
#cartesian = rospy.get_param('~cartesian', True)
arm = MoveGroupCommander('manipulator')
arm.set_pose_reference_frame('base_link')
arm.allow_replanning(True)
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.001)
# arm.set_max_acceleration_scaling_factor(0.5)
#arm.set_max_velocity_scaling_factor(0.5)
end_effector_link = arm.get_end_effector_link()
#arm.set_named_target('home')
arm.set_named_target('up')
arm.go()
rospy.sleep(2)
target_pose = PoseStamped()
target_pose.header.frame_id = 'base_link'
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = 0.86
target_pose.pose.position.y = 0.25
target_pose.pose.position.z = 0.02832
target_pose.pose.orientation.x = 0
target_pose.pose.orientation.y = 0
target_pose.pose.orientation.z = 0
target_pose.pose.orientation.w = 1
#next: find workspace
arm.set_start_state_to_current_state()
arm.set_pose_target(target_pose, end_effector_link)
traj = arm.plan()
arm.execute(traj)
#arm.shift_pose_target(2,-0.05,end_effector_link)
#arm.go()
rospy.sleep(2)
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Initialize the move group for the right arm
right_arm = MoveGroupCommander('right_arm')
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Allow some leeway in position(meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.02)
right_arm.set_goal_orientation_tolerance(0.1)
# Start the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('resting')
right_arm.go()
# Move to the named "straight_forward" pose stored in the SRDF file
right_arm.set_named_target('straight_forward')
right_arm.go()
rospy.sleep(2)
# Move back to the resting position at roughly 1/4 speed
right_arm.set_named_target('resting')
# Get back the planned trajectory
traj = right_arm.plan()
# Scale the trajectory speed by a factor of 0.25
new_traj = scale_trajectory_speed(traj, 0.25)
# Execute the new trajectory
right_arm.execute(new_traj)
rospy.sleep(1)
# Exit MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
class MoveCup():
def __init__(self):
#basic initiatioon
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_tutorial')
self.robot = moveit_commander.RobotCommander()
################ Collision Object
self.scene = moveit_commander.PlanningSceneInterface()
table = CollisionObject()
primitive = SolidPrimitive()
primitive.type = primitive.BOX
box_pose = geometry_msgs.msg.PoseStamped()
box_pose.header.stamp = rospy.Time.now()
box_pose.header.frame_id = 'tablelol'
box_pose.pose.position.x = 1.25
box_pose.pose.position.y = 0.0
box_pose.pose.position.z = -0.6
table.primitives.append(primitive)
table.primitive_poses.append(box_pose)
table.operation = table.ADD
self.scene.add_box('table', box_pose, size=(.077, .073, .122))
#use joint_group parameter to change which arm it uses?
self.joint_group = rospy.get_param('~arm', default="left_arm")
self.group = MoveGroupCommander(self.joint_group)
#self.group.set_planner_id("BKPIECEkConfigDefault")
#this node will scale any tf pose requests to be at most max_reach from the base frame
self.max_reach = rospy.get_param('~max_reach', default=1.1)
#define a start pose that we can move to before stuff runs
self.start_pose = PoseStamped()
self.start_pose = self.get_start_pose()
#remove this when working for realz
self.display_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=10)
self.rate = rospy.Rate(1)
self.ik_srv = rospy.ServiceProxy(
'ExternalTools/left/PositionKinematicsNode/IKService',
SolvePositionIK)
self.limb = baxter_interface.Limb('left')
self.rangesub = rospy.Subscriber('cup_center', geometry_msgs.msg.Point,
self.rangecb)
self.stop = False
self.planning = False
def rangecb(self, point):
if self.planning and point.z == 0:
rospy.loginfo('stop')
self.stop = True
self.move_start()
self.rangesub.unregister()
def callback(self, targetarray):
#callback that moves in a constrained path to anything published to /target_poses
##First, scale the position to be withing self.max_reach
#new_target = self.project_point(targetarray.data)
# rospy.loginfo(self.stop)
if not self.stop:
self.planning = True
new_target = self.project_point(targetarray)
target = PoseStamped()
target.header.stamp = rospy.Time.now()
target.header.frame_id = 'base'
target.pose.position = new_target
#change orientation to be upright
target.pose.orientation = self.start_pose.pose.orientation
#clear group info and set it again
self.group.clear_pose_targets()
# self.group.set_path_constraints(self.get_constraint())
self.group.set_planning_time(10)
# self.group.set_pose_target(target)
################### Try joint space planning
jt_state = JointState()
jt_state.header.stamp = rospy.Time.now()
angles = self.limb.joint_angles()
jt_state.name = list(angles.keys())
jt_state.position = list(angles.values())
jt_state.header.frame_id = 'base'
result = self.ik_srv([target], [jt_state], 0)
angles = {}
i = 0
for name in result.joints[0].name:
angles[name] = result.joints[0].position[i]
i = i + 1
self.group.set_joint_value_target(angles)
#plan and execute plan. If I find a way, I should add error checking her
#currently, if the plan fails, it just doesn't move and waits for another pose to be published
plan = self.group.plan()
self.group.execute(plan)
self.rate.sleep()
return
def scale_movegroup(self, vel=.9, acc=.9):
#slows down baxters arm so we stop getting all those velocity limit errors
self.group.set_max_velocity_scaling_factor(vel)
self.group.set_max_acceleration_scaling_factor(acc)
def unscale_movegroup(self):
self.group.set_max_velocity_scaling_factor(1)
self.group.set_max_acceleration_scaling_factor(1)
def start_baxter_interface(self):
#I copied this from an example but have no idea what it does
self._pub_rate = rospy.Publisher('robot/joint_state_publish_rate',
UInt16,
queue_size=10)
self._left_arm = baxter_interface.limb.Limb("left")
self._left_joint_names = self._left_arm.joint_names()
print(self._left_arm.endpoint_pose())
self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
# set joint state publishing to 100Hz
self._pub_rate.publish(100)
return
def get_start_pose(self, point=[.9, 0.2, 0], rpy=[0, math.pi / 2, 0]):
#define a starting position for the move_start method
new_p = PoseStamped()
new_p.header.frame_id = self.robot.get_planning_frame()
new_p.pose.position.x = point[0]
new_p.pose.position.y = point[1]
new_p.pose.position.z = point[2]
p_orient = tf.transformations.quaternion_from_euler(
rpy[0], rpy[1], rpy[2])
p_orient = Quaternion(*p_orient)
new_p.pose.orientation = p_orient
return (new_p)
def project_point(self, multiarray):
#scales an array and returns a point (see: Pose.position) to be within self.max_reach
#convert points from sonar ring frame to shoulder frame
x = multiarray.data[2] + sr[0] - lls[0]
y = multiarray.data[0] + sr[1] - lls[1]
z = (-1 * multiarray.data[1]) + sr[2] - lls[2]
#scale point to a finite reach distance from the shoulder
obj_dist = math.sqrt(x**2 + y**2 + z**2)
scale_val = min(self.max_reach / obj_dist, .99)
point_scaled = Point()
#scale point and bring into the base frames
point_scaled.x = scale_val * x + lls[0]
point_scaled.y = scale_val * y + lls[1]
point_scaled.z = scale_val * z + lls[2]
return (point_scaled)
def move_random(self):
#moves baxter to a random position. used for testing
randstate = PoseStamped()
randstate = self.group.get_random_pose()
self.group.clear_pose_targets()
self.group.set_pose_target(randstate)
self.group.set_planning_time(8)
self.scale_movegroup()
plan = self.group.plan()
while len(
plan.joint_trajectory.points) == 1 and not rospy.is_shutdown():
print('plan no work')
plan = self.group.plan()
self.group.execute(plan)
self.rate.sleep()
return
def move_random_constrained(self):
#move baxter to a random position with constrained path planning. also for testing
self.scale_movegroup()
randstate = PoseStamped()
randstate = self.group.get_random_pose()
self.group.clear_pose_targets()
self.group.set_pose_target(randstate)
self.group.set_path_constraints(self.get_constraint())
self.group.set_planning_time(100)
self.scale_movegroup()
constrained_plan = self.group.plan()
self.group.execute(constrained_plan)
self.unscale_movegroup()
rospy.sleep(3)
return
def move_start(self):
#move baxter to the self.start_pose pose
self.group.clear_pose_targets()
self.group.set_pose_target(self.start_pose)
self.group.set_planning_time(10)
print('moving to start')
plan = self.group.plan()
self.group.execute(plan)
print('at start')
self.rate.sleep()
return
def get_constraint(self,
euler_orientation=[0, math.pi / 2, 0],
tol=[3, 3, .5]):
#method takes euler-angle inputs, this converts it to a quaternion
q_orientation = tf.transformations.quaternion_from_euler(
euler_orientation[0], euler_orientation[1], euler_orientation[2])
orientation_msg = Quaternion(q_orientation[0], q_orientation[1],
q_orientation[2], q_orientation[3])
#defines a constraint that sets the end-effector so a cup in it's hand will be upright, or straight upside-down
constraint = Constraints()
constraint.name = 'upright_wrist'
upright_orientation = OrientationConstraint()
upright_orientation.link_name = self.group.get_end_effector_link()
upright_orientation.orientation = orientation_msg
upright_orientation.absolute_x_axis_tolerance = tol[0]
upright_orientation.absolute_y_axis_tolerance = tol[1]
upright_orientation.absolute_z_axis_tolerance = tol[2]
upright_orientation.weight = 1.0
upright_orientation.header = self.start_pose.header
constraint.orientation_constraints.append(upright_orientation)
return (constraint)
'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)))
client.send_goal(msg)
client.wait_for_result()
# open
open_hand()
# reach
rospy.loginfo("reach")
arm.set_pose_target([0.90, 0.16, 0.255, 0, 0, 0])
arm.plan() and arm.go()
arm.plan() and arm.go()
# approach
rospy.loginfo("approach")
arm.set_pose_target([1.13, 0.16, 0.255, 0, 0, 0])
arm.plan() and arm.go()
# rotate
for i in range(4):
# close
rospy.loginfo("close")
close_hand()
# rotate
angles = arm.get_current_joint_values()
import numpy
start_angle = angles[5]
print("current angles=", start_angle)
class MoveItDemo:
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
#Initialize robot
robot = moveit_commander.RobotCommander()
# Use the planning scene object to add or remove objects
self.scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10)
# Create a publisher for displaying object frames
self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10)
### Create a publisher for visualizing direction ###
self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the MoveIt! commander for the arm
self.right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the MoveIt! commander for the gripper
self.right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# eel = len(self.right_arm.get_end_effector_link())
# print eel
# Allow 5 seconds per planning attempt
# self.right_arm.set_planning_time(5)
# Prepare Action Controller for gripper
self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction)
self.ac.wait_for_server()
# Give the scene a chance to catch up
rospy.sleep(2)
# Prepare Gazebo Subscriber
self.pwh = None
self.pwh_copy = None
self.idx_targ = None
self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)
# self.m_error = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)
### OPEN THE GRIPPER ###
self.open_gripper()
### Attach / Remove Object ###
self.aro = None
# PREPARE THE SCENE
while self.pwh is None:
rospy.sleep(0.05)
# Run and keep in the BG the scene generator also add the ability to kill the code with ctrl^c
timerThread = threading.Thread(target=self.scene_generator)
timerThread.daemon = True
timerThread.start()
### GIVE SCENE TIME TO CATCH UP ###
rospy.sleep(5)
print "==================== Executing ==========================="
blist = ['target','custom_2','custom_3', 'custom_table']
self.idx_list = []
for name in obj_id:
print name
if name not in blist:
self.idx_list.append(obj_id.index(name))
################################## TESTING AREA #####################################
# ga = self.grasp_attempt()
# print ga
# print obj_id
success = False
i=0
while success == False:
idx = self.idx_list[i]
print idx
new_pose, ds = self.declutter_scene(idx)
if ds == True:
self.post_grasp(new_pose, obj_id[idx])
i+=1
################# GRASPING ATTEMPTS ###################
# ga = self.grasp_attempt()
# print ga
rospy.sleep(5)
# # Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# # Exit the script
moveit_commander.os._exit(0)
################################################################# FUNCTIONS #################################################################################
def grasp_attempt(self):
init_poses = []
grasp_poses = []
for axis in range(0,5):
pg = self.grasp_pose(obj_pose[obj_id.index('target')], axis, 'pg')
gp = self.grasp_pose(obj_pose[obj_id.index('target')], axis, 'gp')
init_poses.append(pg)
grasp_poses.append(gp)
gid, grasps = self.grasp_generator(grasp_poses)
for i in range(0, len(gid)):
self.gripper_pose_pub.publish(grasps[i])
rospy.sleep(0.1)
success = False
for pg in range(0,5):
print pg
plp = self.right_arm.plan(init_poses[pg].pose)
print len(plp.joint_trajectory.points)
if len(plp.joint_trajectory.points) == 0:
print "No valid pregrasp Position, continuing on next one"
continue
self.right_arm.plan(init_poses[pg].pose)
self.right_arm.go()
if pg == 0:
idx = gid.index('front')
elif pg == 1:
idx = gid.index('right')
elif pg == 2:
idx = gid.index('left')
elif pg == 3:
idx = gid.index('topx')
else:
idx = gid.index('topy')
print ("idx = ",idx)
for g in range(idx, len(gid)): ### TODO: fix the loop here, we dont want to check every single grasp after the index
print g
pl2 = self.right_arm.plan(grasps[g].pose)
print len(pl2.joint_trajectory.points)
if len(pl2.joint_trajectory.points) == 0:
print "No Valid Grasp, continuing on next one"
continue
self.right_arm.plan(grasps[g].pose)
self.right_arm.go()
success = True
break
if success == True:
break
return success
def declutter_scene(self,index):
print obj_id[index]
init_poses = []
grasp_poses = []
for axis in range(0,5):
pg = self.grasp_pose(obj_pose[index], axis, 'pg')
gp = self.grasp_pose(obj_pose[index], axis, 'gp')
init_poses.append(pg)
grasp_poses.append(gp)
gid, grasps = self.grasp_generator(grasp_poses)
for i in range(0, len(gid)):
self.gripper_pose_pub.publish(grasps[i])
rospy.sleep(0.1)
success = False
for pg in range(0,5):
print pg
plp = self.right_arm.plan(init_poses[pg].pose)
print len(plp.joint_trajectory.points)
if len(plp.joint_trajectory.points) == 0:
print "No valid pregrasp Position, continuing on next one"
continue
self.right_arm.plan(init_poses[pg].pose)
self.right_arm.go()
if pg == 0:
idx = gid.index('front')
elif pg == 1:
idx = gid.index('right')
elif pg == 2:
idx = gid.index('left')
elif pg == 3:
idx = gid.index('topx')
else:
idx = gid.index('topy')
print ("idx = ",idx)
for g in range(idx, len(gid)): ### TODO: fix the loop here, we dont want to check every single grasp after the index
print g
pl2 = self.right_arm.plan(grasps[g].pose)
print len(pl2.joint_trajectory.points)
if len(pl2.joint_trajectory.points) == 0:
print "No Valid Grasp, continuing on next one"
continue
self.right_arm.plan(grasps[g].pose)
self.right_arm.go()
new_pose = grasps[g]
success = True
break
if success == True:
break
return (new_pose,success)
def post_grasp(self,new_pose, obj_to_grasp):
######### GRASP OBJECT/ REMOVE FROM SCENE ######.
self.close_gripper()
self.aro = obj_to_grasp
print self.aro
# ### POST GRASP RETREAT ###
# M1 = transformations.quaternion_matrix([new_pose.pose.orientation.x, new_pose.pose.orientation.y, new_pose.pose.orientation.z, new_pose.pose.orientation.w])
# M1[0,3] = new_pose.pose.position.x
# M1[1,3] = new_pose.pose.position.y
# M1[2,3] = new_pose.pose.position.z
# M2 = transformations.euler_matrix(0, 0, 0)
# M2[0,3] = 0.0 # offset about x
# M2[1,3] = 0.0 # about y
# M2[2,3] = 0.3 # about z
# T1 = np.dot(M1, M2)
# npo = deepcopy(new_pose)
# npo.pose.position.x = T1[0,3]
# npo.pose.position.y = T1[1,3]
# npo.pose.position.z = T1[2,3]
# quat = transformations.quaternion_from_matrix(T1)
# npo.pose.orientation.x = quat[0]
# npo.pose.orientation.y = quat[1]
# npo.pose.orientation.z = quat[2]
# npo.pose.orientation.w = quat[3]
# npo.header.frame_id = REFERENCE_FRAME
# self.right_arm.plan(npo.pose)
# self.right_arm.go()
### PLACE THE OBJECT
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.80
place_pose.pose.position.y = -0.33
place_pose.pose.position.z = 0.53
place_pose.pose.orientation.w = 1.0
def grasp_pose(self, target_pose, axis, stage):
############ TODO : GENERATE AUTOMATED PRE-GRASPING POSITIONS BASED ON THE PRIMITIVE #########
if axis == 0:
M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M1[0,3] = target_pose.pose.position.x
M1[1,3] = target_pose.pose.position.y
M1[2,3] = target_pose.pose.position.z
M2 = transformations.euler_matrix(0, 0, 0)
if stage == 'pg':
M2[0,3] = -0.25 # offset about x
elif stage == 'gp':
M2[0,3] = -0.18 # offset about x
M2[1,3] = 0.0 # about y
M2[2,3] = 0.0 # about z
elif axis == 1:
M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M1[0,3] = target_pose.pose.position.x
M1[1,3] = target_pose.pose.position.y
M1[2,3] = target_pose.pose.position.z
M2 = transformations.euler_matrix(0, 0, 1.57)
M2[0,3] = 0.0 # offset about x
if stage == 'pg':
M2[1,3] = -0.25 # about y
elif stage == 'gp':
M2[1,3] = -0.18 # about y
M2[2,3] = 0.0 # about z
elif axis ==2:
M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M1[0,3] = target_pose.pose.position.x
M1[1,3] = target_pose.pose.position.y
M1[2,3] = target_pose.pose.position.z
M2 = transformations.euler_matrix(0, 0, -1.57)
M2[0,3] = 0.0 # offset about x
if stage == 'pg':
M2[1,3] = 0.25 # about y
elif stage == 'gp':
M2[1,3] = 0.18 # about y
M2[2,3] = 0.0 # about z
elif axis ==3:
M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M1[0,3] = target_pose.pose.position.x
M1[1,3] = target_pose.pose.position.y
M1[2,3] = target_pose.pose.position.z
M2 = transformations.euler_matrix(0, 1.57, 0)
M2[0,3] = 0.0 # offset about x
M2[1,3] = 0.0 # about y
if stage == 'pg':
M2[2,3] = 0.30 # about z
elif stage == 'gp':
M2[2,3] = 0.23 # about z
else:
M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M1[0,3] = target_pose.pose.position.x
M1[1,3] = target_pose.pose.position.y
M1[2,3] = target_pose.pose.position.z
M2 = transformations.euler_matrix(1.57, 1.57, 0)
M2[0,3] = 0.0 # offset about x
M2[1,3] = 0.0 # about y
if stage == 'pg':
M2[2,3] = 0.30 # about z
elif stage == 'gp':
M2[2,3] = 0.23 # about z
T1 = np.dot(M1, M2)
grasp_pose = deepcopy(target_pose)
grasp_pose.pose.position.x = T1[0,3]
grasp_pose.pose.position.y = T1[1,3]
grasp_pose.pose.position.z = T1[2,3]
quat = transformations.quaternion_from_matrix(T1)
grasp_pose.pose.orientation.x = quat[0]
grasp_pose.pose.orientation.y = quat[1]
grasp_pose.pose.orientation.z = quat[2]
grasp_pose.pose.orientation.w = quat[3]
grasp_pose.header.frame_id = REFERENCE_FRAME
return grasp_pose
def grasp_generator(self, initial_poses):
# A list to hold the grasps
grasps = []
o = [] # Original Pose of the object (o)
O=[]
gid = []
i= 0
while i < len(initial_poses):
o.append(initial_poses[i])
i+=1
G = transformations.euler_matrix(0, 0, 0)
# Generate a grasps for along z axis (x and y directions)
k = 0
while k <= 4:
O.append(transformations.quaternion_matrix([o[k].pose.orientation.x, o[k].pose.orientation.y, o[k].pose.orientation.z, o[k].pose.orientation.w]))
O[k][0,3] = o[k].pose.position.x
O[k][1,3] = o[k].pose.position.y
O[k][2,3] = o[k].pose.position.z
if k in range(0,3):
for z in self.drange(-obj_size[obj_id.index('target')][2]/2, obj_size[obj_id.index('target')][2]/2, 0.02):
# print z
T = np.dot(O[k], G)
grasp = deepcopy(o[k])
grasp.pose.position.x = T[0,3]
grasp.pose.position.y = T[1,3]
grasp.pose.position.z = T[2,3] +z
quat = transformations.quaternion_from_matrix(T)
grasp.pose.orientation.x = quat[0]
grasp.pose.orientation.y = quat[1]
grasp.pose.orientation.z = quat[2]
grasp.pose.orientation.w = quat[3]
grasp.header.frame_id = REFERENCE_FRAME
# Append the grasp to the list
grasps.append(deepcopy(grasp))
if k ==0:
gid.append('front')
elif k ==1:
gid.append('right')
elif k ==2:
gid.append('left')
elif k == 3:
for x in self.drange(-obj_size[obj_id.index('target')][1]/2, obj_size[obj_id.index('target')][1]/2, 0.01):
# print z
T = np.dot(O[k], G)
grasp = deepcopy(o[k])
grasp.pose.position.x = T[0,3] +x
grasp.pose.position.y = T[1,3]
grasp.pose.position.z = T[2,3]
quat = transformations.quaternion_from_matrix(T)
grasp.pose.orientation.x = quat[0]
grasp.pose.orientation.y = quat[1]
grasp.pose.orientation.z = quat[2]
grasp.pose.orientation.w = quat[3]
grasp.header.frame_id = REFERENCE_FRAME
# Append the grasp to the list
grasps.append(deepcopy(grasp))
gid.append('topx')
else:
for y in self.drange(-obj_size[obj_id.index('target')][1]/2, obj_size[obj_id.index('target')][1]/2, 0.01):
# print z
T = np.dot(O[k], G)
grasp = deepcopy(o[k])
grasp.pose.position.x = T[0,3]
grasp.pose.position.y = T[1,3] +y
grasp.pose.position.z = T[2,3]
quat = transformations.quaternion_from_matrix(T)
grasp.pose.orientation.x = quat[0]
grasp.pose.orientation.y = quat[1]
grasp.pose.orientation.z = quat[2]
grasp.pose.orientation.w = quat[3]
grasp.header.frame_id = REFERENCE_FRAME
# Append the grasp to the list
grasps.append(deepcopy(grasp))
gid.append('topy')
k+=1
# Return the list
return (gid,grasps)
def scene_generator(self):
obj_pose =[]
obj_id = []
obj_size = []
bl = ['ground_plane','pr2']
global obj_pose, obj_id , obj_size
ops = PoseStamped()
ops.header.frame_id = REFERENCE_FRAME
for model_name in self.pwh.name:
if model_name not in bl:
obj_id.append(model_name)
ops.pose = self.pwh.pose[self.pwh.name.index(model_name)]
obj_pose.append(deepcopy(ops))
obj_size.append([0.05, 0.05, 0.15])
obj_id[obj_id.index('custom_1')] = 'target'
obj_size[obj_id.index('custom_2')] = [0.05, 0.05, 0.10]
obj_size[obj_id.index('custom_3')] = [0.05, 0.05, 0.05]
obj_size[obj_id.index('custom_table')] = [1.5, 0.8, 0.03]
### REMOVE OBJECT FROM PREVIOUS RUNS ###
for i in range(0, len(obj_id)):
self.scene.remove_world_object(obj_id[i])
# Remove any attached objects from a previous session
self.scene.remove_attached_object(GRIPPER_FRAME, obj_id[obj_id.index('target')])
next_call = time.time()
while True:
next_call = next_call+1
if self.aro is None:
for i in range(0, len(obj_id)):
### CREATE THE SCENE ###
self.scene.add_box(obj_id[i], obj_pose[i], obj_size[i])
### Make the target purple and obstacles orange ###
self.setColor(obj_id[obj_id.index('target')], 0.6, 0, 1, 1.0)
self.setColor(obj_id[obj_id.index('custom_2')], 1, 0.623, 0, 1.0)
self.setColor(obj_id[obj_id.index('custom_3')], 1, 0.623, 0, 1.0)
self.setColor(obj_id[obj_id.index('custom_1_0')], 1, 0.623, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
else:
###### ATTACH ITEM TO GRIPPER ######
touch_links = [GRIPPER_FRAME, 'r_gripper_l_finger_tip_link','r_gripper_r_finger_tip_link', 'r_gripper_r_finger_link', 'r_gripper_l_finger_link']
#print touch_links
self.scene.attach_box(GRIPPER_FRAME, obj_id[self.aro], obj_pose[self.aro], obj_size[self.aro], touch_links)
### REMOVE SPECIFIC OBJECT AFTER IT HAS BEEN ATTACHED TO GRIPPER ###
self.scene.remove_world_object(obj_id[self.aro])
time.sleep(next_call - time.time())
def model_state_callback(self,msg):
self.pwh = ModelStates()
self.pwh = msg
def drange(self, start, stop, step):
r = start
while r < stop:
yield r
r += step
def close_gripper(self):
g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.035, 100))
self.ac.send_goal(g_close)
self.ac.wait_for_result()
rospy.sleep(15) # Gazebo requires up to 15 seconds to attach object
def open_gripper(self):
g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100))
self.ac.send_goal(g_open)
self.ac.wait_for_result()
rospy.sleep(5) # And up to 20 to detach it
# Set the color of an object
def setColor(self, name, r, g, b, a = 0.9):
# Initialize a MoveIt color object
color = ObjectColor()
# Set the id to the name given as an argument
color.id = name
# Set the rgb and alpha values given as input
color.color.r = r
color.color.g = g
color.color.b = b
color.color.a = a
# Update the global color dictionary
self.colors[name] = color
# Actually send the colors to MoveIt!
def sendColors(self):
# Initialize a planning scene object
p = PlanningScene()
# Need to publish a planning scene diff
p.is_diff = True
# Append the colors from the global color dictionary
for color in self.colors.values():
p.object_colors.append(color)
# Publish the scene diff
self.scene_pub.publish(p)
rospy.loginfo("Right Arm Pose Target 1:\n{}".format(pose_target_rarm_1))
pose_target_larm_1 = Pose()
pose_target_larm_1.position.x = pose_target_rarm_1.position.x
pose_target_larm_1.position.y = pose_target_rarm_1.position.y * -1.0
pose_target_larm_1.position.z = pose_target_rarm_1.position.z
pose_target_larm_1.orientation.x = pose_target_rarm_1.orientation.x
pose_target_larm_1.orientation.y = pose_target_rarm_1.orientation.y
pose_target_larm_1.orientation.z = pose_target_rarm_1.orientation.z
pose_target_larm_1.orientation.w = pose_target_rarm_1.orientation.w
rospy.loginfo("Left Arm Pose Target 1:\n{}".format(pose_target_larm_1))
group.set_pose_target(pose_target_rarm_1, 'RARM_JOINT5_Link')
group.set_pose_target(pose_target_larm_1, 'LARM_JOINT5_Link')
group.plan()
group.go()
# Pose Target 2
pose_target_rarm_2 = Pose()
pose_target_rarm_2.position.x = 0.3
pose_target_rarm_2.position.y = -0.3
pose_target_rarm_2.position.z = 0.5
pose_target_rarm_2.orientation.y = -1.0
rospy.loginfo("Right Arm Pose Target 2:\n{}".format(pose_target_rarm_2))
pose_target_larm_2 = Pose()
pose_target_larm_2.position.x = pose_target_rarm_2.position.x
pose_target_larm_2.position.y = pose_target_rarm_2.position.y * -1.0
pose_target_larm_2.position.z = pose_target_rarm_2.position.z
class SmartGrasper(object):
"""
This is the helper library to easily access the different functionalities of the simulated robot
from python.
"""
__last_joint_state = None
__current_model_name = "cricket_ball"
__path_to_models = "/root/.gazebo/models/"
def __init__(self):
"""
This constructor initialises the different necessary connections to the topics and services
and resets the world to start in a good position.
"""
rospy.init_node("smart_grasper")
self.__joint_state_sub = rospy.Subscriber("/joint_states", JointState,
self.__joint_state_cb, queue_size=1)
rospy.wait_for_service("/gazebo/get_model_state", 10.0)
rospy.wait_for_service("/gazebo/reset_world", 10.0)
self.__reset_world = rospy.ServiceProxy("/gazebo/reset_world", Empty)
self.__get_pose_srv = rospy.ServiceProxy("/gazebo/get_model_state", GetModelState)
rospy.wait_for_service("/gazebo/pause_physics")
self.__pause_physics = rospy.ServiceProxy("/gazebo/pause_physics", Empty)
rospy.wait_for_service("/gazebo/unpause_physics")
self.__unpause_physics = rospy.ServiceProxy("/gazebo/unpause_physics", Empty)
rospy.wait_for_service("/controller_manager/switch_controller")
self.__switch_ctrl = rospy.ServiceProxy("/controller_manager/switch_controller", SwitchController)
rospy.wait_for_service("/gazebo/set_model_configuration")
self.__set_model = rospy.ServiceProxy("/gazebo/set_model_configuration", SetModelConfiguration)
rospy.wait_for_service("/gazebo/delete_model")
self.__delete_model = rospy.ServiceProxy("/gazebo/delete_model", DeleteModel)
rospy.wait_for_service("/gazebo/spawn_sdf_model")
self.__spawn_model = rospy.ServiceProxy("/gazebo/spawn_sdf_model", SpawnModel)
rospy.wait_for_service('/get_planning_scene', 10.0)
self.__get_planning_scene = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene)
self.__pub_planning_scene = rospy.Publisher('/planning_scene', PlanningScene, queue_size=10, latch=True)
self.arm_commander = MoveGroupCommander("arm")
self.hand_commander = MoveGroupCommander("hand")
self.__hand_traj_client = SimpleActionClient("/hand_controller/follow_joint_trajectory",
FollowJointTrajectoryAction)
self.__arm_traj_client = SimpleActionClient("/arm_controller/follow_joint_trajectory",
FollowJointTrajectoryAction)
if self.__hand_traj_client.wait_for_server(timeout=rospy.Duration(4.0)) is False:
rospy.logfatal("Failed to connect to /hand_controller/follow_joint_trajectory in 4sec.")
raise Exception("Failed to connect to /hand_controller/follow_joint_trajectory in 4sec.")
if self.__arm_traj_client.wait_for_server(timeout=rospy.Duration(4.0)) is False:
rospy.logfatal("Failed to connect to /arm_controller/follow_joint_trajectory in 4sec.")
raise Exception("Failed to connect to /arm_controller/follow_joint_trajectory in 4sec.")
self.reset_world()
def reset_world(self):
"""
Resets the object poses in the world and the robot joint angles.
"""
self.__switch_ctrl.call(start_controllers=[],
stop_controllers=["hand_controller", "arm_controller", "joint_state_controller"],
strictness=SwitchControllerRequest.BEST_EFFORT)
self.__pause_physics.call()
joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint', 'H1_F1J1', 'H1_F1J2', 'H1_F1J3',
'H1_F2J1', 'H1_F2J2', 'H1_F2J3', 'H1_F3J1', 'H1_F3J2', 'H1_F3J3']
joint_positions = [1.2, 0.3, -1.5, -0.5, -1.5, 0.0, 0.0, -0.3, 0.0, 0.0, -0.3, 0.0, 0.0, -0.3, 0.0]
self.__set_model.call(model_name="smart_grasping_sandbox",
urdf_param_name="robot_description",
joint_names=joint_names,
joint_positions=joint_positions)
timer = Timer(0.0, self.__start_ctrl)
timer.start()
time.sleep(0.1)
self.__unpause_physics.call()
self.__reset_world.call()
def get_object_pose(self):
"""
Gets the pose of the ball in the world frame.
@return The pose of the ball.
"""
return self.__get_pose_srv.call(self.__current_model_name, "world").pose
def get_tip_pose(self):
"""
Gets the current pose of the robot's tooltip in the world frame.
@return the tip pose
"""
return self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose
def move_tip_absolute(self, target):
"""
Moves the tooltip to the absolute target in the world frame
@param target is a geometry_msgs.msg.Pose
@return True on success
"""
self.arm_commander.set_start_state_to_current_state()
self.arm_commander.set_pose_targets([target])
plan = self.arm_commander.plan()
if not self.arm_commander.execute(plan):
return False
return True
def move_tip(self, x=0., y=0., z=0., roll=0., pitch=0., yaw=0.):
"""
Moves the tooltip in the world frame by the given x,y,z / roll,pitch,yaw.
@return True on success
"""
transform = PyKDL.Frame(PyKDL.Rotation.RPY(pitch, roll, yaw),
PyKDL.Vector(-x, -y, -z))
tip_pose = self.get_tip_pose()
tip_pose_kdl = posemath.fromMsg(tip_pose)
final_pose = toMsg(tip_pose_kdl * transform)
self.arm_commander.set_start_state_to_current_state()
self.arm_commander.set_pose_targets([final_pose])
plan = self.arm_commander.plan()
if not self.arm_commander.execute(plan):
return False
return True
def send_command(self, command, duration=0.2):
"""
Send a dictionnary of joint targets to the arm and hand directly.
@param command: a dictionnary of joint names associated with a target:
{"H1_F1J1": -1.0, "shoulder_pan_joint": 1.0}
@param duration: the amount of time it will take to get there in seconds. Needs to be bigger than 0.0
"""
hand_goal = None
arm_goal = None
for joint, target in command.items():
if "H1" in joint:
if not hand_goal:
hand_goal = FollowJointTrajectoryGoal()
point = JointTrajectoryPoint()
point.time_from_start = rospy.Duration.from_sec(duration)
hand_goal.trajectory.points.append(point)
hand_goal.trajectory.joint_names.append(joint)
hand_goal.trajectory.points[0].positions.append(target)
else:
if not arm_goal:
arm_goal = FollowJointTrajectoryGoal()
point = JointTrajectoryPoint()
point.time_from_start = rospy.Duration.from_sec(duration)
arm_goal.trajectory.points.append(point)
arm_goal.trajectory.joint_names.append(joint)
arm_goal.trajectory.points[0].positions.append(target)
if arm_goal:
self.__arm_traj_client.send_goal_and_wait(arm_goal)
if hand_goal:
self.__hand_traj_client.send_goal_and_wait(hand_goal)
def get_current_joint_state(self):
"""
Gets the current state of the robot.
@return joint positions, velocity and efforts as three dictionnaries
"""
joints_position = {n: p for n, p in
zip(self.__last_joint_state.name,
self.__last_joint_state.position)}
joints_velocity = {n: v for n, v in
zip(self.__last_joint_state.name,
self.__last_joint_state.velocity)}
joints_effort = {n: v for n, v in
zip(self.__last_joint_state.name,
self.__last_joint_state.effort)}
return joints_position, joints_velocity, joints_effort
def open_hand(self):
"""
Opens the hand.
@return True on success
"""
self.hand_commander.set_named_target("open")
plan = self.hand_commander.plan()
if not self.hand_commander.execute(plan, wait=True):
return False
return True
def close_hand(self):
"""
Closes the hand.
@return True on success
"""
self.hand_commander.set_named_target("close")
plan = self.hand_commander.plan()
if not self.hand_commander.execute(plan, wait=True):
return False
return True
def check_fingers_collisions(self, enable=True):
"""
Disables or enables the collisions check between the fingers and the objects / table
@param enable: set to True to enable / False to disable
@return True on success
"""
objects = ["cricket_ball__link", "drill__link", "cafe_table__link"]
while self.__pub_planning_scene.get_num_connections() < 1:
rospy.loginfo("waiting for someone to subscribe to the /planning_scene")
rospy.sleep(0.1)
request = PlanningSceneComponents(components=PlanningSceneComponents.ALLOWED_COLLISION_MATRIX)
response = self.__get_planning_scene(request)
acm = response.scene.allowed_collision_matrix
for object_name in objects:
if object_name not in acm.entry_names:
# add object to allowed collision matrix
acm.entry_names += [object_name]
for row in range(len(acm.entry_values)):
acm.entry_values[row].enabled += [False]
new_row = deepcopy(acm.entry_values[0])
acm.entry_values += {new_row}
for index_entry_values, entry_values in enumerate(acm.entry_values):
if "H1_F" in acm.entry_names[index_entry_values]:
for index_value, _ in enumerate(entry_values.enabled):
if acm.entry_names[index_value] in objects:
if enable:
acm.entry_values[index_entry_values].enabled[index_value] = False
else:
acm.entry_values[index_entry_values].enabled[index_value] = True
elif acm.entry_names[index_entry_values] in objects:
for index_value, _ in enumerate(entry_values.enabled):
if "H1_F" in acm.entry_names[index_value]:
if enable:
acm.entry_values[index_entry_values].enabled[index_value] = False
else:
acm.entry_values[index_entry_values].enabled[index_value] = True
planning_scene_diff = PlanningScene(is_diff=True, allowed_collision_matrix=acm)
self.__pub_planning_scene.publish(planning_scene_diff)
rospy.sleep(1.0)
return True
def pick(self):
"""
Does its best to pick the ball.
"""
rospy.loginfo("Moving to Pregrasp")
self.open_hand()
time.sleep(0.1)
ball_pose = self.get_object_pose()
ball_pose.position.z += 0.5
#setting an absolute orientation (from the top)
quaternion = quaternion_from_euler(-pi/2., 0.0, 0.0)
ball_pose.orientation.x = quaternion[0]
ball_pose.orientation.y = quaternion[1]
ball_pose.orientation.z = quaternion[2]
ball_pose.orientation.w = quaternion[3]
self.move_tip_absolute(ball_pose)
time.sleep(0.1)
rospy.loginfo("Grasping")
self.move_tip(y=-0.164)
time.sleep(0.1)
self.check_fingers_collisions(False)
time.sleep(0.1)
self.close_hand()
time.sleep(0.1)
rospy.loginfo("Lifting")
for _ in range(5):
self.move_tip(y=0.01)
time.sleep(0.1)
self.check_fingers_collisions(True)
def swap_object(self, new_model_name):
"""
Replaces the current object with a new one.Replaces
@new_model_name the name of the folder in which the object is (e.g. beer)
"""
try:
self.__delete_model(self.__current_model_name)
except:
rospy.logwarn("Failed to delete: " + self.__current_model_name)
try:
sdf = None
initial_pose = Pose()
initial_pose.position.x = 0.15
initial_pose.position.z = 0.82
with open(self.__path_to_models + new_model_name + "/model.sdf", "r") as model:
sdf = model.read()
res = self.__spawn_model(new_model_name, sdf, "", initial_pose, "world")
rospy.logerr( "RES: " + str(res) )
self.__current_model_name = new_model_name
except:
rospy.logwarn("Failed to delete: " + self.__current_model_name)
def __compute_arm_target_for_ball(self):
ball_pose = self.get_object_pose()
# come at it from the top
arm_target = ball_pose
arm_target.position.z += 0.5
quaternion = quaternion_from_euler(-pi/2., 0.0, 0.0)
arm_target.orientation.x = quaternion[0]
arm_target.orientation.y = quaternion[1]
arm_target.orientation.z = quaternion[2]
arm_target.orientation.w = quaternion[3]
return arm_target
def __pre_grasp(self, arm_target):
self.hand_commander.set_named_target("open")
plan = self.hand_commander.plan()
self.hand_commander.execute(plan, wait=True)
for _ in range(10):
self.arm_commander.set_start_state_to_current_state()
self.arm_commander.set_pose_targets([arm_target])
plan = self.arm_commander.plan()
if self.arm_commander.execute(plan):
return True
def __grasp(self, arm_target):
waypoints = []
waypoints.append(self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose)
arm_above_ball = deepcopy(arm_target)
arm_above_ball.position.z -= 0.12
waypoints.append(arm_above_ball)
self.arm_commander.set_start_state_to_current_state()
(plan, fraction) = self.arm_commander.compute_cartesian_path(waypoints, 0.01, 0.0)
print fraction
if not self.arm_commander.execute(plan):
return False
self.hand_commander.set_named_target("close")
plan = self.hand_commander.plan()
if not self.hand_commander.execute(plan, wait=True):
return False
self.hand_commander.attach_object("cricket_ball__link")
def __lift(self, arm_target):
waypoints = []
waypoints.append(self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose)
arm_above_ball = deepcopy(arm_target)
arm_above_ball.position.z += 0.1
waypoints.append(arm_above_ball)
self.arm_commander.set_start_state_to_current_state()
(plan, fraction) = self.arm_commander.compute_cartesian_path(waypoints, 0.01, 0.0)
print fraction
if not self.arm_commander.execute(plan):
return False
def __start_ctrl(self):
rospy.loginfo("STARTING CONTROLLERS")
self.__switch_ctrl.call(start_controllers=["hand_controller", "arm_controller", "joint_state_controller"],
stop_controllers=[], strictness=1)
def __joint_state_cb(self, msg):
self.__last_joint_state = msg
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)
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,
queue_size=5)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose',
PoseStamped,
queue_size=5)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the move group for the right arm
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# 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.05)
right_arm.set_goal_orientation_tolerance(0.1)
# 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(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
right_arm.set_planning_time(60)
# 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
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
target_id = 'target'
tool_id = 'tool'
# 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)
scene.remove_world_object(target_id)
scene.remove_world_object(tool_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)
# Start the arm in the "grasp" pose stored in the SRDF file
right_arm.set_named_target('right_arm_up')
right_arm.go()
# Open the gripper to the neutral position
right_gripper.set_joint_value_target(GRIPPER_OPEN)
right_gripper.go()
rospy.sleep(5)
# Set the height of the table off the ground
table_ground = 0.04
# Set the dimensions of the scene objects [l, w, h]
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.02, 0.01, 0.12]
target_x = 0.135
#target_y = -0.32
target_y = -0.285290879999
# 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.25
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)
# 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 = target_x
target_pose.pose.position.y = target_y
target_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
target_pose.pose.orientation.w = 1.0
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
# Make the table blue and the boxes orange
self.setColor(table_id, 0, 0, 0.8, 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
right_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 = 0.18
place_pose.pose.position.y = 0
place_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
place_pose.pose.orientation.w = 1.0
p = PoseStamped()
p.header.frame_id = "up1_footprint"
p.pose.position.x = 0.12792118579
p.pose.position.y = -0.285290879999
p.pose.position.z = 0.120301181892
p.pose.orientation.x = 0.0
p.pose.orientation.y = 0.0
p.pose.orientation.z = -0.706825181105
p.pose.orientation.w = 0.707388269167
right_gripper.set_pose_target(p.pose)
# pick an object
right_arm.allow_replanning(True)
right_arm.allow_looking(True)
right_arm.set_goal_tolerance(0.05)
right_arm.set_planning_time(60)
print "arm grasp"
success = 0
attempt = 0
while not success:
p_plan = right_arm.plan()
attempt = attempt + 1
print "Planning attempt: " + str(attempt)
if p_plan.joint_trajectory.points != []:
success = 1
print "arm grasp"
right_arm.execute(p_plan)
rospy.sleep(5)
right_gripper.set_joint_value_target(GRIPPER_GRASP)
right_gripper.go()
print "gripper closed"
rospy.sleep(5)
scene.attach_box(GRIPPER_FRAME, target_id)
print "object attached"
right_arm.set_named_target('right_arm_up')
right_arm.go()
print "arm up"
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
start_pose.position.x = -0.0988490064784
start_pose.position.y = 0.272349904278
start_pose.position.z = 1.18864499931
start_pose.orientation.x = 0.393751611087
start_pose.orientation.y = 0.918424640162
start_pose.orientation.z = -0.0150455838492
start_pose.orientation.w = 0.0350639347048
# start_pose.orientation.w = 0
# start_pose.orientation.x = 0
# start_pose.orientation.y = 1
# start_pose.orientation.z = 0
# start_pose.position.y = 0.0256415233819
# start_pose.position.z = 1.25871460368
# start_pose.position.x = 0.243500142238
right_arm.set_pose_target(start_pose)
plan_start = right_arm.plan()
print "============ Waiting while RVIZ displays plan_start..."
rospy.sleep(5)
right_arm.execute(plan_start)
print "============ Waiting while RVIZ executes plan_start..."
rospy.sleep(5)
waypoints = []
waypoints.append(right_arm.get_current_pose().pose)
gain = 0.2
points = 5
for i in xrange(points):
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = waypoints[i-1].orientation.w
wpose.orientation.x = waypoints[i-1].orientation.x
joint_target_4, joint_target_5, joint_target_init
]
joint_targets = []
for i in range(0, 361, 45):
for j in range(-20, 21, 20):
i_rads = i * math.pi / 180
j_rads = j * math.pi / 180
joint_target = rotate_joint(joint_target_init, i_rads,
JOINT_INDEX_BASE)
joint_target = rotate_joint(joint_target, j_rads,
JOINT_INDEX_UPPER_ARM)
joint_targets.append(joint_target)
for joint_target in joint_targets:
group.set_joint_value_target(joint_target)
group.set_max_velocity_scaling_factor(0.1)
plan = group.plan()
group.go(wait=True)
group.stop()
if rospy.is_shutdown():
break
rospy.sleep(1)
roscpp_shutdown()
class Planner(object):
move_group = None
goals = None
jspub = None
namespace = None
# These will eventually go to model objects
robot_data = {
'group_name': 'right_arm_and_torso',
'eef_link': 'r_wrist_joint_link'
}
# Current state of the 'session' (right now, only one)
current_scene = None
status = None
link_poses = None
def __init__(self):
rospy.init_node('moveit_web',disable_signals=True)
self.jspub = rospy.Publisher('/update_joint_states',JointState)
self.psw_pub = rospy.Publisher('/planning_scene_world', PlanningSceneWorld)
# Give time for subscribers to connect to the publisher
rospy.sleep(1)
self.goals = []
# HACK: Synthesize a valid initial joint configuration for PR2
initial_joint_state = JointState()
initial_joint_state.name = ['r_elbow_flex_joint']
initial_joint_state.position = [-0.1]
self.jspub.publish(initial_joint_state)
# Create group we'll use all along this demo
# self.move_group = MoveGroupCommander('right_arm_and_torso')
self.move_group = MoveGroupCommander(self.robot_data['group_name'])
self._move_group = self.move_group._g
self.ps = PlanningSceneInterface()
self.status = {'text':'ready to plan','ready':True}
def get_scene(self):
return self.current_scene
def set_scene(self, scene):
self.current_scene = scene
psw = PlanningSceneWorld()
for co_json in scene['objects']:
# TODO: Fix orientation by using proper quaternions on the client
pose = self._make_pose(co_json['pose'])
# TODO: Decide what to do with STL vs. Collada. The client has a Collada
# loader but the PlanningSceneInterface can only deal with STL.
# TODO: Proper mapping between filenames and URLs
# filename = '/home/julian/aaad/moveit/src/moveit_web/django%s' % co_json['meshUrl']
filename = '/home/julian/aaad/moveit/src/moveit_web/django/static/meshes/table_4legs.stl'
co = self.ps.make_mesh(co_json['name'], pose, filename)
psw.collision_objects.append(co)
self.psw_pub.publish(psw)
def get_link_poses(self):
if self.link_poses is None:
self.link_poses = self._move_group.get_link_poses_compressed()
return self.link_poses
# Create link back to socket.io namespace to allow emitting information
def set_socket(self, namespace):
self.namespace = namespace
def emit(self, event, data=None):
if self.namespace:
self.namespace.emit(event, data)
def emit_new_goal(self, pose):
self.emit('target_pose', message_converter.convert_ros_message_to_dictionary(pose)['pose'])
def set_random_goal(self):
goal_pose = self.move_group.get_random_pose()
# goal_pose = self.move_group.get_random_pose('base_footprint')
self.emit_new_goal(goal_pose)
def _make_pose(self, json_pose):
pose = PoseStamped()
pose.header.frame_id = "odom_combined"
pp = json_pose['position']
pose.pose.position.x = pp['x']
pose.pose.position.y = pp['y']
pose.pose.position.z = pp['z']
# TODO: Orientation is not working. See about
# properly using Quaternions everywhere
pp = json_pose['orientation']
pose.pose.orientation.x = pp['x']
pose.pose.orientation.y = pp['y']
pose.pose.orientation.z = pp['z']
pose.pose.orientation.w = pp['w']
return pose
def plan_to_poses(self, poses):
goal_pose = self._make_pose(poses[0])
self.move_group.set_pose_target(goal_pose)
# self.move_group.set_pose_target(goal_pose,'base_footprint')
self.emit('status',{'text':'Starting to plan'})
trajectory = self.move_group.plan()
if trajectory is None or len(trajectory.joint_trajectory.joint_names) == 0:
self.status = {'reachable':False,'text':'Ready to plan','ready':True}
self.emit('status', self.status)
else:
self.status = {'reachable':True,'text':'Rendering trajectory'}
self.emit('status', self.status)
self.publish_trajectory(trajectory)
def publish_goal_position(self, trajectory):
self.publish_position(self, trajectory, -1)
def publish_position(self, trajectory, step):
jsmsg = JointState()
jsmsg.name = trajectory.joint_trajectory.joint_names
jsmsg.position = trajectory.joint_trajectory.points[step].positions
self.jspub.publish(jsmsg)
def publish_trajectory(self, trajectory):
cur_time = 0.0
acceleration = 4.0
for i in range(len(trajectory.joint_trajectory.points)):
point = trajectory.joint_trajectory.points[i]
gevent.sleep((point.time_from_start - cur_time)/acceleration)
cur_time = point.time_from_start
# self.publish_position(trajectory, i)
# TODO: Only say "True" to update state on the last step of the trajectory
new_poses = self._move_group.update_robot_state(trajectory.joint_trajectory.joint_names,
trajectory.joint_trajectory.points[i].positions, True)
self.link_poses = new_poses
self.emit('link_poses', new_poses)
self.status = {'text':'Ready to plan','ready':True}
self.emit('status', self.status)
class MoveItDemo:
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
#Initialize robot
robot = moveit_commander.RobotCommander()
# Use the planning scene object to add or remove objects
self.scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10)
# Create a publisher for displaying object frames
self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10)
### Create a publisher for visualizing direction ###
self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the MoveIt! commander for the arm
self.right_arm = MoveGroupCommander(GROUP_NAME_ARM)
self.left_arm = MoveGroupCommander('left_arm')
# Initialize the MoveIt! commander for the gripper
self.right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
self.left_gripper = MoveGroupCommander('left_gripper')
# eel = len(self.right_arm.get_end_effector_link())
# print eel
# Allow 5 seconds per planning attempt
# self.right_arm.set_planning_time(5)
# Prepare Action Controller for gripper
self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction)
self.ac.wait_for_server()
# Give the scene a chance to catch up
rospy.sleep(2)
# Prepare Gazebo Subscriber
self.pwh = None
self.pwh_copy = None
self.idx_targ = None
self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)
### OPEN THE GRIPPER ###
self.open_gripper()
# PREPARE THE SCENE
while self.pwh is None:
rospy.sleep(0.05)
### Attach / Remove Object Flag ###
self.aro = None
# Run and keep in the BG the scene generator with ctrl^c kill ###
timerThread = threading.Thread(target=self.scene_generator)
timerThread.daemon = True
timerThread.start()
## Give some time to ensure the thread starts!! ##
rospy.sleep(5)
### GENERATE THE BLACKLIST AND REMOVE ATTACHED OBJECTS FROM PREVIOUS RUNS ###
self.idx_list = self.bl()
### GIVE SCENE TIME TO CATCH UP ###
rospy.sleep(5)
################################## GRASP EXECUTION #####################################
print "==================== Executing ==========================="
start_time = time.time()
### PERSONAL REMINDER!!! WHAT IS WHAT!!! ###
# print obj_id[obj_id.index('target')]
# print obj_id.index('target')
### MOVE LEFT ARM OUT OF THE WAY ###
self.lasp()
success = False
while success == False and len(self.idx_list)>0:
success, pgr_target = self.grasp_attempt()
print ("GA Returns:", success)
if success is not False:
self.flag = 0 # To let the planning scene know when to remove the object
self.post_grasp(pgr_target, obj_id.index('target'),'true')
self.place_object(obj_id.index('target'))
break
else:
idx = self.idx_list[0]
ds, pgr_col_obj = self.declutter_scene(idx)
print ("DS Returns:", ds)
if ds == True:
self.flag = 0 # To let the planning scene know when to remove the object
self.post_grasp(pgr_col_obj, obj_id.index(obj_id[idx]),'true')
self.place_object(obj_id.index(obj_id[idx]))
self.idx_list.pop(0)
print "==================== THE END! ======================"
print("--- %s seconds ---" % (time.time() - start_time))
rospy.sleep(5)
# # Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# # Exit the script
moveit_commander.os._exit(0)
################################################################# FUNCTIONS #################################################################################
def grasp_attempt(self):
# start_time = time.time()
retreat = None
init_poses = []
grasp_poses = []
for axis in range(0,6):
# while obj_id[obj_id.index('target')] is not 'target':
# print '!!!!!'
# rospy.sleep(0.05)
pg = self.grasp_pose(obj_pose[obj_id.index('target')], axis, 'pg')
gp = self.grasp_pose(obj_pose[obj_id.index('target')], axis, 'gp')
init_poses.append(pg)
grasp_poses.append(gp)
pre_grasps = self.grasp_generator(init_poses)
grasps = self.grasp_generator(grasp_poses)
for grasp in grasps:
self.gripper_pose_pub.publish(grasp)
rospy.sleep(0.05)
success = False
i = 1
for pg, gr in izip(pre_grasps, grasps):
self.gripper_pose_pub.publish(gr)
print ("G Attempt: ", i)
plp = self.right_arm.plan(pg.pose)
if len(plp.joint_trajectory.points) == 0:
print "No valid pregrasp Position, continuing on next one"
i+=1
continue
i+=1
self.right_arm.plan(pg.pose)
self.right_arm.go(wait=True)
rospy.sleep(5)
plg = self.right_arm.plan(gr.pose)
if len(plg.joint_trajectory.points) >= 10:
self.right_arm.go()
success = True
retreat = gr
print "Grasping"
break
# print("--- %s seconds ---" % (time.time() - start_time))
return success , retreat
def declutter_scene(self,index):
retreat = None
init_poses = []
grasp_poses = []
for axis in range(0,6):
pg = self.grasp_pose(obj_pose[index], axis, 'pg')
gp = self.grasp_pose(obj_pose[index], axis, 'gp')
init_poses.append(pg)
grasp_poses.append(gp)
pre_grasps = self.grasp_generator(init_poses)
grasps = self.grasp_generator(grasp_poses)
for grasp in grasps:
self.gripper_pose_pub.publish(grasp)
rospy.sleep(0.05)
success = False
i= 1
for pg, gr in izip(pre_grasps, grasps):
plp = self.right_arm.plan(pg.pose)
print (" DS Attempt: ", i)
self.gripper_pose_pub.publish(gr)
self.right_arm.plan(pg.pose)
if len(plp.joint_trajectory.points) == 0:
print "No valid pregrasp Position, continuing on next one"
i+=1
continue
i+=1
self.right_arm.plan(pg.pose)
self.right_arm.go()
rospy.sleep(5)
plg = self.right_arm.plan(gr.pose)
if len(plg.joint_trajectory.points) >= 10:
self.right_arm.go()
print "Grasping"
success = True
retreat = gr
break
return success, retreat
def place_object(self, obj_idx):
self.aro = obj_idx
### GENERATE PLACE POSES ###
places = self.place_generator()
### TRY THESE POSES ###
i = 1
for place in places:
print (" Place Attempt: ", i)
plpl = self.right_arm.plan(place.pose)
print len(plpl.joint_trajectory.points)
if len(plpl.joint_trajectory.points) == 0:
i+=1
continue
self.right_arm.plan(plpl)
self.right_arm.go(wait=True)
### INFORM SCENE ###
# self.open_gripper()
# self.aro = None
### RETURN HAND TO STARTING POSITION ###
self.post_grasp(place,obj_idx, 'false')
self.rasp()
break
def post_grasp(self,new_pose, obj_idx, fl):
######### GRASP OBJECT/ REMOVE FROM SCENE ######.
if fl == 'true':
self.close_gripper()
self.aro = obj_idx
else:
self.open_gripper()
self.aro = None
rospy.sleep(2)
### POST GRASP RETREAT ###
M1 = transformations.quaternion_matrix([new_pose.pose.orientation.x, new_pose.pose.orientation.y, new_pose.pose.orientation.z, new_pose.pose.orientation.w])
M1[0,3] = new_pose.pose.position.x
M1[1,3] = new_pose.pose.position.y
M1[2,3] = new_pose.pose.position.z
M2 = transformations.euler_matrix(0, 0, 0)
M2[0,3] = 0.0 # offset about x
M2[1,3] = 0.0 # about y
M2[2,3] = 0.25 # about z
T1 = np.dot(M2, M1)
npo = deepcopy(new_pose)
npo.pose.position.x = T1[0,3]
npo.pose.position.y = T1[1,3]
npo.pose.position.z = T1[2,3]
quat = transformations.quaternion_from_matrix(T1)
npo.pose.orientation.x = quat[0]
npo.pose.orientation.y = quat[1]
npo.pose.orientation.z = quat[2]
npo.pose.orientation.w = quat[3]
npo.header.frame_id = REFERENCE_FRAME
self.right_arm.plan(npo.pose)
self.right_arm.go(wait=True)
def place_generator(self):
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.57
place_pose.pose.position.y = 0.16
place_pose.pose.position.z = 0.56
place_pose.pose.orientation.w = 1.0
P = transformations.quaternion_matrix([place_pose.pose.orientation.x, place_pose.pose.orientation.y, place_pose.pose.orientation.z, place_pose.pose.orientation.w])
P[0,3] = place_pose.pose.position.x
P[1,3] = place_pose.pose.position.y
P[2,3] = place_pose.pose.position.z
places =[]
yaw_angles = [0, 1,57, -1,57 , 3,14]
x_vals = [0, 0.05 ,0.1 , 0.15]
z_vals = [0.05 ,0.1 , 0.15]
for y in yaw_angles:
G = transformations.euler_matrix(0, 0, y)
G[0,3] = 0.0 # offset about x
G[1,3] = 0.0 # about y
G[2,3] = 0.0 # about z
for z in z_vals:
for x in x_vals:
TM = np.dot(P, G)
pl = deepcopy(place_pose)
pl.pose.position.x = TM[0,3] +x
pl.pose.position.y = TM[1,3]
pl.pose.position.z = TM[2,3] +z
quat = transformations.quaternion_from_matrix(TM)
pl.pose.orientation.x = quat[0]
pl.pose.orientation.y = quat[1]
pl.pose.orientation.z = quat[2]
pl.pose.orientation.w = quat[3]
pl.header.frame_id = REFERENCE_FRAME
places.append(deepcopy(pl))
return places
def grasp_pose(self, target_pose, axis, stage):
############ TODO : GENERATE AUTOMATED PRE-GRASPING POSITIONS BASED ON THE PRIMITIVE #########
if axis == 0:
M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M1[0,3] = target_pose.pose.position.x
M1[1,3] = target_pose.pose.position.y
M1[2,3] = target_pose.pose.position.z
M2 = transformations.euler_matrix(0, 0, 0)
if stage == 'pg':
M2[0,3] = -0.25 # offset about x
elif stage == 'gp':
M2[0,3] = -0.18 # offset about x
M2[1,3] = 0.0 # about y
M2[2,3] = 0.0 # about z
elif axis == 1:
M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M1[0,3] = target_pose.pose.position.x
M1[1,3] = target_pose.pose.position.y
M1[2,3] = target_pose.pose.position.z
M2 = transformations.euler_matrix(0, 0, 1.57)
M2[0,3] = 0.0 # offset about x
if stage == 'pg':
M2[1,3] = -0.25 # about y
elif stage == 'gp':
M2[1,3] = -0.18 # about y
M2[2,3] = 0.0 # about z
elif axis == 2:
M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M1[0,3] = target_pose.pose.position.x
M1[1,3] = target_pose.pose.position.y
M1[2,3] = target_pose.pose.position.z
M2 = transformations.euler_matrix(0, 0, -1.57)
M2[0,3] = 0.0 # offset about x
if stage == 'pg':
M2[1,3] = 0.25 # about y
elif stage == 'gp':
M2[1,3] = 0.18 # about y
M2[2,3] = 0.0 # about z
elif axis == 3:
M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M1[0,3] = target_pose.pose.position.x
M1[1,3] = target_pose.pose.position.y
M1[2,3] = target_pose.pose.position.z
M2 = transformations.euler_matrix(0, 0, 3.14)
if stage == 'pg':
M2[0,3] = 0.25 # offset about x
elif stage == 'gp':
M2[0,3] = 0.18 # offset about x
M2[1,3] = 0.0 # about y
M2[2,3] = 0.0 # about z
elif axis == 4:
M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M1[0,3] = target_pose.pose.position.x
M1[1,3] = target_pose.pose.position.y
M1[2,3] = target_pose.pose.position.z
M2 = transformations.euler_matrix(0, 1.57, 0)
M2[0,3] = 0.0 # offset about x
M2[1,3] = 0.0 # about y
if stage == 'pg':
M2[2,3] = 0.30 # about z
elif stage == 'gp':
M2[2,3] = 0.23 # about z
else:
M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M1[0,3] = target_pose.pose.position.x
M1[1,3] = target_pose.pose.position.y
M1[2,3] = target_pose.pose.position.z
M2 = transformations.euler_matrix(1.57, 1.57, 0)
M2[0,3] = 0.0 # offset about x
M2[1,3] = 0.0 # about y
if stage == 'pg':
M2[2,3] = 0.30 # about z
elif stage == 'gp':
M2[2,3] = 0.23 # about z
T1 = np.dot(M1, M2)
grasp_pose = deepcopy(target_pose)
grasp_pose.pose.position.x = T1[0,3]
grasp_pose.pose.position.y = T1[1,3]
grasp_pose.pose.position.z = T1[2,3]
quat = transformations.quaternion_from_matrix(T1)
grasp_pose.pose.orientation.x = quat[0]
grasp_pose.pose.orientation.y = quat[1]
grasp_pose.pose.orientation.z = quat[2]
grasp_pose.pose.orientation.w = quat[3]
grasp_pose.header.frame_id = REFERENCE_FRAME
return grasp_pose
def grasp_generator(self, initial_poses):
# A list to hold the grasps
grasps = []
o = [] # Original Pose of the object (o)
O=[]
i= 0
while i < len(initial_poses):
o.append(initial_poses[i])
i+=1
G = transformations.euler_matrix(0, 0, 0)
# Generate a grasps for along z axis (x and y directions)
k = 0
while k <= 5:
O.append(transformations.quaternion_matrix([o[k].pose.orientation.x, o[k].pose.orientation.y, o[k].pose.orientation.z, o[k].pose.orientation.w]))
O[k][0,3] = o[k].pose.position.x
O[k][1,3] = o[k].pose.position.y
O[k][2,3] = o[k].pose.position.z
if k in range(0,4):
for z in self.drange(0.005-obj_size[obj_id.index('target')][2]/2,-0.005 + obj_size[obj_id.index('target')][2]/2, 0.02): ### TODO: USE EACH OBJECTS SIZE NOT ONLY THE TARGETS ###
# print z
T = np.dot(O[k], G)
grasp = deepcopy(o[k])
grasp.pose.position.x = T[0,3]
grasp.pose.position.y = T[1,3]
grasp.pose.position.z = T[2,3] +z
quat = transformations.quaternion_from_matrix(T)
grasp.pose.orientation.x = quat[0]
grasp.pose.orientation.y = quat[1]
grasp.pose.orientation.z = quat[2]
grasp.pose.orientation.w = quat[3]
grasp.header.frame_id = REFERENCE_FRAME
# Append the grasp to the list
grasps.append(deepcopy(grasp))
elif k == 4:
for x in self.drange(-obj_size[obj_id.index('target')][1]/2, obj_size[obj_id.index('target')][1]/2, 0.02):
# print z
T = np.dot(O[k], G)
grasp = deepcopy(o[k])
grasp.pose.position.x = T[0,3] +x
grasp.pose.position.y = T[1,3]
grasp.pose.position.z = T[2,3]
quat = transformations.quaternion_from_matrix(T)
grasp.pose.orientation.x = quat[0]
grasp.pose.orientation.y = quat[1]
grasp.pose.orientation.z = quat[2]
grasp.pose.orientation.w = quat[3]
grasp.header.frame_id = REFERENCE_FRAME
# Append the grasp to the list
grasps.append(deepcopy(grasp))
else:
for y in self.drange(-obj_size[obj_id.index('target')][1]/2, obj_size[obj_id.index('target')][1]/2, 0.02):
# print z
T = np.dot(O[k], G)
grasp = deepcopy(o[k])
grasp.pose.position.x = T[0,3]
grasp.pose.position.y = T[1,3] +y
grasp.pose.position.z = T[2,3]
quat = transformations.quaternion_from_matrix(T)
grasp.pose.orientation.x = quat[0]
grasp.pose.orientation.y = quat[1]
grasp.pose.orientation.z = quat[2]
grasp.pose.orientation.w = quat[3]
grasp.header.frame_id = REFERENCE_FRAME
# Append the grasp to the list
grasps.append(deepcopy(grasp))
k+=1
print len(grasps)
# Return the list
return grasps
def scene_generator(self):
while True:
# print "happening"
obj_pose =[]
obj_id = []
obj_size = []
bl = ['ground_plane','pr2']
global obj_pose, obj_id , obj_size
ops = PoseStamped()
ops.header.frame_id = REFERENCE_FRAME
for model_name in self.pwh.name:
if model_name not in bl:
obj_id.append(model_name)
ops.pose = self.pwh.pose[self.pwh.name.index(model_name)]
obj_pose.append(deepcopy(ops))
obj_size.append([0.05, 0.05, 0.15])
# obj_id[obj_id.index('custom_1')] = 'target'
obj_size[obj_id.index('custom_2')] = [0.05, 0.05, 0.10]
obj_size[obj_id.index('custom_3')] = [0.05, 0.05, 0.05]
obj_size[obj_id.index('custom_table')] = [1.5, 0.8, 0.03]
if self.aro is None:
for i in range(0, len(obj_id)):
### CREATE THE SCENE ###
self.scene.add_box(obj_id[i], obj_pose[i], obj_size[i])
self.setColor(obj_id[i], 1, 0.623, 0, 1.0)
### Make the target purple and table green ###
self.setColor(obj_id[obj_id.index('target')], 0.6, 0, 1, 1.0)
self.setColor(obj_id[obj_id.index('custom_table')], 0.3, 1, 0.3, 1.0)
self.scene.remove_attached_object(GRIPPER_FRAME)
# Send the colors to the planning scene
self.sendColors()
else:
if self.flag == 0:
touch_links = [GRIPPER_FRAME, 'r_gripper_l_finger_tip_link','r_gripper_r_finger_tip_link', 'r_gripper_r_finger_link', 'r_gripper_l_finger_link', 'r_wrist_roll_link', 'r_upper_arm_link']
#print touch_links
self.scene.attach_box(GRIPPER_FRAME, obj_id[self.aro], obj_pose[self.aro], obj_size[self.aro], touch_links)
### REMOVE SPECIFIC OBJECT AFTER IT HAS BEEN ATTACHED TO GRIPPER ###
self.scene.remove_world_object(obj_id[self.aro])
self.flag +=1
time.sleep(0.5)
def model_state_callback(self,msg):
self.pwh = ModelStates()
self.pwh = msg
def bl(self):
blist = ['target','custom_2','custom_3', 'custom_table']
self.blist = []
for name in obj_id:
if name not in blist:
self.blist.append(obj_id.index(name))
# Remove any attached objects from a previous session
self.scene.remove_attached_object(GRIPPER_FRAME, obj_id[obj_id.index(name)])
self.scene.remove_attached_object(GRIPPER_FRAME, 'target')
return self.blist
def drange(self, start, stop, step):
r = start
while r < stop:
yield r
r += step
def close_gripper(self):
g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.044, 100))
self.ac.send_goal(g_close)
self.ac.wait_for_result()
rospy.sleep(15) # Gazebo requires up to 15 seconds to attach object
def open_gripper(self):
g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.0899, 100))
self.ac.send_goal(g_open)
self.ac.wait_for_result()
rospy.sleep(5) # And up to 20 to detach it
# Set the color of an object
def setColor(self, name, r, g, b, a = 0.9):
# Initialize a MoveIt color object
color = ObjectColor()
# Set the id to the name given as an argument
color.id = name
# Set the rgb and alpha values given as input
color.color.r = r
color.color.g = g
color.color.b = b
color.color.a = a
# Update the global color dictionary
self.colors[name] = color
# Actually send the colors to MoveIt!
def sendColors(self):
# Initialize a planning scene object
p = PlanningScene()
# Need to publish a planning scene diff
p.is_diff = True
# Append the colors from the global color dictionary
for color in self.colors.values():
p.object_colors.append(color)
# Publish the scene diff
self.scene_pub.publish(p)
def lasp(self):
sp = PoseStamped()
sp.header.frame_id = REFERENCE_FRAME
sp.pose.position.x = 0.3665
sp.pose.position.y = 0.74094
sp.pose.position.z = 1.1449
sp.pose.orientation.x = 0.80503
sp.pose.orientation.y = -0.18319
sp.pose.orientation.z = 0.31988
sp.pose.orientation.w = 0.46481
self.left_arm.plan(sp)
self.left_arm.go(wait=True)
def rasp(self):
sp = PoseStamped()
sp.header.frame_id = REFERENCE_FRAME
sp.pose.position.x = 0.39571
sp.pose.position.y = -0.40201
sp.pose.position.z = 1.1128
sp.pose.orientation.x =0.00044829
sp.pose.orientation.y = 0.57956
sp.pose.orientation.z = 9.4878e-05
sp.pose.orientation.w = 0.81493
self.right_arm.plan(sp)
self.right_arm.go(wait=True)
mg = MoveGroupCommander('right_arm_and_torso')
p = mg.get_current_pose()
print "Start pose:"
print p
p.pose.position.x += 0.3
#ps = PlanningSceneInterface()
#psw_pub = rospy.Publisher('/planning_scene_world', PlanningSceneWorld)
#rospy.sleep(1)
#co = ps.make_sphere("test_co", p, 0.02)
#psw = PlanningSceneWorld()
#psw.collision_objects.append(co)
#psw_pub.publish(psw)
# ps.remove_world_object("test_sphere")
# ps.add_sphere("test_sphere", p, 0.1)
# rospy.sleep(1)
# ps.add_sphere("test_sphere", p, 0.1)
#p.pose.position.x += 0.3
print "End pose:"
print p
mg.set_pose_target(mg.get_random_pose())
traj = mg.plan()
print traj
print ">>>>> Printing robot state"
print robot.get_current_state()
print ">>>>> Printing robot pose"
print group.get_current_pose()
## Planning to a Pose goal
print ">>>>> Generating plan"
pose_target = geometry_msgs.msg.Pose()
#pose_target.orientation.w = 1.0
pose_target.position.x = 0.5 #0.4
pose_target.position.y = 0.2 #0.2
pose_target.position.z = 0.2 #0.2
group.set_pose_target(pose_target)
plan = group.plan()
#print "============ Waiting while RVIZ displays plan..."
rospy.sleep(1)
print ">>>>> Go for plan"
group.go(wait=True)
## Adding/Removing Objects and Attaching/Detaching Objects
collision_object = moveit_msgs.msg.CollisionObject()
moveit_commander.roscpp_shutdown()
rospy.spin()
roscpp_shutdown()
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
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))
rospy.init_node('pumpkin_planning', anonymous=True)
right_arm = MoveGroupCommander("right_arm")
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=1)
print right_arm.get_current_pose().pose
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = 0.0176318609849
wpose.orientation.x = 0.392651866792
wpose.orientation.y = 0.918465607415
wpose.orientation.z = 0.0439835989663
wpose.position.y = 0.227115629827
wpose.position.z = 1.32344046934
wpose.position.x = -0.358178766726
right_arm.set_pose_target(wpose)
plan1 = right_arm.plan()
right_arm.execute(plan1)
print "============ Waiting while RVIZ executes plan1..."
rospy.sleep(5)
waypoints = []
waypoints.append(right_arm.get_current_pose().pose)
print right_arm.get_current_pose().pose
points = 20
for i in xrange(points):
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = waypoints[i-1].orientation.w + 0.5285
wpose.orientation.x = waypoints[i-1].orientation.x - 0.6736
wpose.orientation.y = waypoints[i-1].orientation.y - 1.638
wpose.orientation.z = waypoints[i-1].orientation.z - 0.308
rospy.init_node('pumpkin_planning', anonymous=True)
right_arm = MoveGroupCommander("right_arm")
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=1)
print right_arm.get_current_pose().pose
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = 0.56984725052
wpose.orientation.x = -0.279552201843
wpose.orientation.y = -0.720240690777
wpose.orientation.z = -0.279960755942
wpose.position.y = 0.109418655245
wpose.position.z = 1.32343676946
wpose.position.x = 0.230101774989
right_arm.set_pose_target(wpose)
plan1 = right_arm.plan()
right_arm.execute(plan1)
print "============ Waiting while RVIZ executes plan1..."
rospy.sleep(5)
waypoints = []
waypoints.append(right_arm.get_current_pose().pose)
print right_arm.get_current_pose().pose
points = 20
for i in xrange(points):
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = waypoints[i-1].orientation.w - 0.5285
wpose.orientation.x = waypoints[i-1].orientation.x + 0.6736
wpose.orientation.y = waypoints[i-1].orientation.y + 1.638
class MoveItDemo:
def __init__(self):
global obj_att
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
#Initialize robot
robot = moveit_commander.RobotCommander()
# Use the planning scene object to add or remove objects
self.scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10)
# Create a publisher for displaying object frames
self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10)
### Create a publisher for visualizing direction ###
self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the MoveIt! commander for the arm
self.right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the MoveIt! commander for the gripper
self.right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Allow 5 seconds per planning attempt
self.right_arm.set_planning_time(5)
# Prepare Action Controller for gripper
self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction)
self.ac.wait_for_server()
# Give the scene a chance to catch up
rospy.sleep(2)
# Prepare Gazebo Subscriber
self.pwh = None
self.pwh_copy = None
self.idx_targ = None
self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)
### OPEN THE GRIPPER ###
self.open_gripper()
# PREPARE THE SCENE
while self.pwh is None:
rospy.sleep(0.05)
############## CLEAR THE SCENE ################
# planning_scene.world.collision_objects.remove('target')
# Remove leftover objects from a previous run
self.scene.remove_world_object('target')
self.scene.remove_world_object('table')
# self.scene.remove_world_object(obstacle1_id)
# Remove any attached objects from a previous session
self.scene.remove_attached_object(GRIPPER_FRAME, 'target')
# Run and keep in the BG the scene generator also add the ability to kill the code with ctrl^c
timerThread = threading.Thread(target=self.scene_generator)
timerThread.daemon = True
timerThread.start()
initial_pose = PoseStamped()
initial_pose.header.frame_id = 'gazebo_world'
initial_pose.pose = target_pose.pose
print "==================== Generating Transformations ==========================="
#################### PRE GRASPING POSE #########################
M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M1[0,3] = target_pose.pose.position.x
M1[1,3] = target_pose.pose.position.y
M1[2,3] = target_pose.pose.position.z
M2 = transformations.euler_matrix(0, 1.57, 0)
M2[0,3] = 0.0 # offset about x
M2[1,3] = 0.0 # about y
M2[2,3] = 0.25 # about z
T = np.dot(M1, M2)
pre_grasping = deepcopy(target_pose)
pre_grasping.pose.position.x = T[0,3]
pre_grasping.pose.position.y = T[1,3]
pre_grasping.pose.position.z = T[2,3]
quat = transformations.quaternion_from_matrix(T)
pre_grasping.pose.orientation.x = quat[0]
pre_grasping.pose.orientation.y = quat[1]
pre_grasping.pose.orientation.z = quat[2]
pre_grasping.pose.orientation.w = quat[3]
pre_grasping.header.frame_id = 'gazebo_world'
self.plan_exec(pre_grasping)
#################### GRASPING POSE #########################
M3 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M3[0,3] = target_pose.pose.position.x
M3[1,3] = target_pose.pose.position.y
M3[2,3] = target_pose.pose.position.z
M4 = transformations.euler_matrix(0, 1.57, 0)
M4[0,3] = 0.0 # offset about x
M4[1,3] = 0.0 # about y
M4[2,3] = 0.18 # about z
T2 = np.dot(M3, M4)
grasping = deepcopy(target_pose)
grasping.pose.position.x = T2[0,3]
grasping.pose.position.y = T2[1,3]
grasping.pose.position.z = T2[2,3]
quat2 = transformations.quaternion_from_matrix(T2)
grasping.pose.orientation.x = quat2[0]
grasping.pose.orientation.y = quat2[1]
grasping.pose.orientation.z = quat2[2]
grasping.pose.orientation.w = quat2[3]
grasping.header.frame_id = 'gazebo_world'
self.plan_exec(grasping)
#Close the gripper
print "========== Waiting for gazebo to catch up =========="
self.close_gripper()
#################### ATTACH OBJECT ######################
touch_links = [GRIPPER_FRAME, 'r_gripper_l_finger_tip_link','r_gripper_r_finger_tip_link', 'r_gripper_r_finger_link', 'r_gripper_l_finger_link']
#print touch_links
self.scene.attach_box(GRIPPER_FRAME, target_id, target_pose, target_size, touch_links)
# counter to let the planning scene know when to remove the object
obj_att = 1
#self.scene.remove_world_object(target_id)
#################### POST-GRASP RETREAT #########################
M5 = transformations.quaternion_matrix([initial_pose.pose.orientation.x, initial_pose.pose.orientation.y, initial_pose.pose.orientation.z, initial_pose.pose.orientation.w])
M5[0,3] = initial_pose.pose.position.x
M5[1,3] = initial_pose.pose.position.y
M5[2,3] = initial_pose.pose.position.z
M6 = transformations.euler_matrix(0, 1.57, 0)
M6[0,3] = 0.0 # offset about x
M6[1,3] = 0.0 # about y
M6[2,3] = 0.3 # about z
T3 = np.dot(M5, M6)
post_grasping = deepcopy(initial_pose)
post_grasping.pose.position.x = T3[0,3]
post_grasping.pose.position.y = T3[1,3]
post_grasping.pose.position.z = T3[2,3]
quat3 = transformations.quaternion_from_matrix(T3)
post_grasping.pose.orientation.x = quat3[0]
post_grasping.pose.orientation.y = quat3[1]
post_grasping.pose.orientation.z = quat3[2]
post_grasping.pose.orientation.w = quat3[3]
post_grasping.header.frame_id = 'gazebo_world'
self.plan_exec(post_grasping)
# 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 = 0.52
place_pose.pose.position.y = -0.48
place_pose.pose.position.z = 0.48
place_pose.pose.orientation.w = 1.0
n_attempts = 0
max_place_attempts = 2
# Generate valid place poses
places = self.make_places(place_pose)
success = False
# Repeat until we succeed or run out of attempts
while success == False and n_attempts < max_place_attempts:
for place in places:
success = self.right_arm.place(target_id, place)
if success:
break
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
rospy.sleep(0.2)
self.open_gripper()
obj_att = None
rospy.sleep(3)
## # Initialize the grasp object
## g = Grasp()
## grasps = []
## # Set the first grasp pose to the input pose
## g.grasp_pose = pre_grasping
## g.allowed_touch_objects = [target_id]
## grasps.append(deepcopy(g))
## right_arm.pick(target_id, grasps)
# #Change the frame_id for the planning to take place!
# #target_pose.header.frame_id = 'gazebo_world'
# # Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# # Exit the script
moveit_commander.os._exit(0)
##################################################################################################################
#Get pose from Gazebo
def model_state_callback(self,msg):
self.pwh = ModelStates()
self.pwh = msg
# Generate a list of possible place poses
def make_places(self, init_pose):
# Initialize the place location as a PoseStamped message
place = PoseStamped()
# Start with the input place pose
place = init_pose
# A list of x shifts (meters) to try
x_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.015]
# A list of y shifts (meters) to try
y_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.015]
# A list of pitch angles to try
#pitch_vals = [0, 0.005, -0.005, 0.01, -0.01, 0.02, -0.02]
pitch_vals = [0]
# A list of yaw angles to try
yaw_vals = [0]
# A list to hold the places
places = []
# Generate a place pose for each angle and translation
for y in yaw_vals:
for p in pitch_vals:
for y in y_vals:
for x in x_vals:
place.pose.position.x = init_pose.pose.position.x + x
place.pose.position.y = init_pose.pose.position.y + y
# Create a quaternion from the Euler angles
q = quaternion_from_euler(0, p, y)
# Set the place pose orientation accordingly
place.pose.orientation.x = q[0]
place.pose.orientation.y = q[1]
place.pose.orientation.z = q[2]
place.pose.orientation.w = q[3]
# Append this place pose to the list
places.append(deepcopy(place))
# Return the list
return places
def plan_exec(self, pose):
self.right_arm.clear_pose_targets()
self.right_arm.set_pose_target(pose, GRIPPER_FRAME)
self.right_arm.plan()
rospy.sleep(5)
self.right_arm.go(wait=True)
def close_gripper(self):
g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.035, 100))
self.ac.send_goal(g_close)
self.ac.wait_for_result()
rospy.sleep(15) # Gazebo requires up to 15 seconds to attach object
def open_gripper(self):
g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100))
self.ac.send_goal(g_open)
self.ac.wait_for_result()
rospy.sleep(5) # And up to 20 to detach it
def scene_generator(self):
# print obj_att
global target_pose
global target_id
global target_size
target_id = 'target'
self.taid = self.pwh.name.index('wood_cube_5cm')
table_id = 'table'
self.tid = self.pwh.name.index('table')
#obstacle1_id = 'obstacle1'
#self.o1id = self.pwh.name.index('wood_block_10_2_1cm')
# Set the target size [l, w, h]
target_size = [0.05, 0.05, 0.05]
table_size = [1.5, 0.8, 0.03]
#obstacle1_size = [0.1, 0.025, 0.01]
## Set the target pose on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose = self.pwh.pose[self.taid]
target_pose.pose.position.z += 0.025
# Add the target object to the scene
if obj_att is None:
self.scene.add_box(target_id, target_pose, target_size)
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose = self.pwh.pose[self.tid]
table_pose.pose.position.z += 1
self.scene.add_box(table_id, table_pose, table_size)
#obstacle1_pose = PoseStamped()
#obstacle1_pose.header.frame_id = REFERENCE_FRAME
#obstacle1_pose.pose = self.pwh.pose[self.o1id]
## Add the target object to the scene
#scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size)
# 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 = 0.50
place_pose.pose.position.y = -0.30
place_pose.pose.orientation.w = 1.0
# Add the target object to the scene
self.scene.add_box(target_id, target_pose, target_size)
### Make the target purple ###
self.setColor(target_id, 0.6, 0, 1, 1.0)
# Send the colors to the planning scene
self.sendColors()
else:
self.scene.remove_world_object('target')
# Publish targe's frame
#self.object_frames_pub.publish(target_pose)
threading.Timer(0.5, self.scene_generator).start()
# Set the color of an object
def setColor(self, name, r, g, b, a = 0.9):
# Initialize a MoveIt color object
color = ObjectColor()
# Set the id to the name given as an argument
color.id = name
# Set the rgb and alpha values given as input
color.color.r = r
color.color.g = g
color.color.b = b
color.color.a = a
# Update the global color dictionary
self.colors[name] = color
# Actually send the colors to MoveIt!
def sendColors(self):
# Initialize a planning scene object
p = PlanningScene()
# Need to publish a planning scene diff
p.is_diff = True
# Append the colors from the global color dictionary
for color in self.colors.values():
p.object_colors.append(color)
# Publish the scene diff
self.scene_pub.publish(p)
# Set the start pose
start_pose = geometry_msgs.msg.Pose(
) # Find how to set to custom pose - right_hand_kinect_start
# Values for right_hand_kinect_start
start_pose.position.x = 0.0464712799415
start_pose.position.y = 0.0737200235193
start_pose.position.z = 1.3721234889
start_pose.orientation.x = 0.212880825044
start_pose.orientation.y = 0.806655581926
start_pose.orientation.z = 0.440611611646
start_pose.orientation.w = 0.331436169057
right_shoulder_to_wrist.set_pose_target(start_pose)
plan_start = right_shoulder_to_wrist.plan(
) #Facing problems in planning with this mouve group
rospy.sleep(5)
right_shoulder_to_wrist.execute(plan_start)
rospy.sleep(5)
# Set the orientation for next pose
next_pose.orientation.x = 0.5
next_pose.orientation.y = 0.5
next_pose.orientation.z = 0.5
next_pose.orientation.w = 0.5
# Subscriber
rospy.Subscriber("/right_shoulder_to_wrist_transform", Float32MultiArray,
right_shoulder_to_wrist_callback)
# Start mimic
class MoveItDemo:
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
#Initialize robot
robot = moveit_commander.RobotCommander()
# Use the planning scene object to add or remove objects
self.scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10)
# Create a publisher for displaying object frames
self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10)
### Create a publisher for visualizing direction ###
self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the MoveIt! commander for the arm
self.right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the MoveIt! commander for the gripper
self.right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Allow 5 seconds per planning attempt
self.right_arm.set_planning_time(5)
# Prepare Action Controller for gripper
self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction)
self.ac.wait_for_server()
# Give the scene a chance to catch up
rospy.sleep(2)
# Prepare Gazebo Subscriber
self.pwh = None
self.pwh_copy = None
self.idx_targ = None
self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)
### OPEN THE GRIPPER ###
self.open_gripper()
self.obj_att = None
# PREPARE THE SCENE
while self.pwh is None:
rospy.sleep(0.05)
############## CLEAR THE SCENE ################
# planning_scene.world.collision_objects.remove('target')
# Remove leftover objects from a previous run
self.scene.remove_world_object('target')
self.scene.remove_world_object('table')
# self.scene.remove_world_object(obstacle1_id)
# Remove any attached objects from a previous session
self.scene.remove_attached_object(GRIPPER_FRAME, 'target')
# Run and keep in the BG the scene generator also add the ability to kill the code with ctrl^c
timerThread = threading.Thread(target=self.scene_generator)
timerThread.daemon = True
timerThread.start()
initial_pose = target_pose
initial_pose.header.frame_id = 'gazebo_world'
print "==================== Generating Transformations ==========================="
#################### PRE GRASPING POSE #########################
# M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
# M1[0,3] = target_pose.pose.position.x
# M1[1,3] = target_pose.pose.position.y
# M1[2,3] = target_pose.pose.position.z
# M2 = transformations.euler_matrix(0, 1.57, 0)
# M2[0,3] = 0.0 # offset about x
# M2[1,3] = 0.0 # about y
# M2[2,3] = 0.25 # about z
# T = np.dot(M1, M2)
# pre_grasping = deepcopy(target_pose)
# pre_grasping.pose.position.x = T[0,3]
# pre_grasping.pose.position.y = T[1,3]
# pre_grasping.pose.position.z = T[2,3]
# quat = transformations.quaternion_from_matrix(T)
# pre_grasping.pose.orientation.x = quat[0]
# pre_grasping.pose.orientation.y = quat[1]
# pre_grasping.pose.orientation.z = quat[2]
# pre_grasping.pose.orientation.w = quat[3]
# pre_grasping.header.frame_id = 'gazebo_world'
# self.plan_exec(pre_grasping)
################# GENERATE GRASPS ###################
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 5
# Track success/failure and number of attempts for pick operation
success = False
n_attempts = 0
grasps = self.grasp_generator(initial_pose)
possible_grasps = []
for grasp in grasps:
self.gripper_pose_pub.publish(grasp)
possible_grasps.append(grasp.pose)
rospy.sleep(0.2)
#print possible_grasps
self.right_arm.pick(target_id, grasps)
# target_name = target_id
# group_name = GROUP_NAME_ARM
# end_effector = GROUP_NAME_GRIPPER
# attached_object_touch_links = ['r_forearm_link']
# #print (target_name, group_name, end_effector)
# PickupGoal(target_name, group_name ,end_effector, possible_grasps )
# # Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# # Exit the script
moveit_commander.os._exit(0)
################################################################# FUNCTIONS #################################################################################
def model_state_callback(self,msg):
self.pwh = ModelStates()
self.pwh = msg
def grasp_generator(self, initial_pose):
# Pitch angles to try
pitch_vals = [1, 1.57,0, 1,2 ]
# Yaw angles to try
yaw_vals = [0]#, 1.57, -1.57]
# A list to hold the grasps
grasps = []
g = PoseStamped()
g.header.frame_id = REFERENCE_FRAME
g.pose = initial_pose.pose
#g.pose.position.z += 0.18
# Generate a grasp for each pitch and yaw angle
for y in yaw_vals:
for p in pitch_vals:
# Create a quaternion from the Euler angles
q = transformations.quaternion_from_euler(0, p, y)
# Set the grasp pose orientation accordingly
g.pose.orientation.x = q[0]
g.pose.orientation.y = q[1]
g.pose.orientation.z = q[2]
g.pose.orientation.w = q[3]
# Append the grasp to the list
grasps.append(deepcopy(g))
# Return the list
return grasps
def plan_exec(self, pose):
self.right_arm.clear_pose_targets()
self.right_arm.set_pose_target(pose, GRIPPER_FRAME)
self.right_arm.plan()
rospy.sleep(5)
self.right_arm.go(wait=True)
def close_gripper(self):
g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.035, 100))
self.ac.send_goal(g_close)
self.ac.wait_for_result()
rospy.sleep(15) # Gazebo requires up to 15 seconds to attach object
def open_gripper(self):
g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100))
self.ac.send_goal(g_open)
self.ac.wait_for_result()
rospy.sleep(5) # And up to 20 to detach it
def scene_generator(self):
# print obj_att
global target_pose
global target_id
next_call = time.time()
while True:
next_call = next_call+1
target_id = 'target'
self.taid = self.pwh.name.index('wood_cube_5cm')
table_id = 'table'
self.tid = self.pwh.name.index('table')
# obstacle1_id = 'obstacle1'
# self.o1id = self.pwh.name.index('wood_block_10_2_1cm')
# Set the target size [l, w, h]
target_size = [0.05, 0.05, 0.05]
table_size = [1.5, 0.8, 0.03]
# obstacle1_size = [0.1, 0.025, 0.01]
## Set the target pose on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose = self.pwh.pose[self.taid]
target_pose.pose.position.z += 0.025
if self.obj_att is None:
# Add the target object to the scene
self.scene.add_box(target_id, target_pose, target_size)
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose = self.pwh.pose[self.tid]
table_pose.pose.position.z += 1
self.scene.add_box(table_id, table_pose, table_size)
# obstacle1_pose = PoseStamped()
# obstacle1_pose.header.frame_id = REFERENCE_FRAME
# obstacle1_pose.pose = self.pwh.pose[self.o1id]
# # Add the target object to the scene
# scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size)
### Make the target purple ###
self.setColor(target_id, 0.6, 0, 1, 1.0)
# Send the colors to the planning scene
self.sendColors()
else:
self.scene.remove_world_object('target')
time.sleep(next_call - time.time())
#threading.Timer(0.5, self.scene_generator).start()
# Set the color of an object
def setColor(self, name, r, g, b, a = 0.9):
# Initialize a MoveIt color object
color = ObjectColor()
# Set the id to the name given as an argument
color.id = name
# Set the rgb and alpha values given as input
color.color.r = r
color.color.g = g
color.color.b = b
color.color.a = a
# Update the global color dictionary
self.colors[name] = color
# Actually send the colors to MoveIt!
def sendColors(self):
# Initialize a planning scene object
p = PlanningScene()
# Need to publish a planning scene diff
p.is_diff = True
# Append the colors from the global color dictionary
for color in self.colors.values():
p.object_colors.append(color)
# Publish the scene diff
self.scene_pub.publish(p)
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")
if __name__ == '__main__':
print "--- Straight line gesture ---"
rospy.init_node('straight_line', anonymous=True)
right_arm = MoveGroupCommander("right_arm")
start_pose = geometry_msgs.msg.Pose()
start_pose.position.x = -0.0206330384032
start_pose.position.y = 0.077582282778
start_pose.position.z = 1.39283677496
start_pose.orientation.x = 0.5
start_pose.orientation.y = 0.5
start_pose.orientation.z = 0.5
start_pose.orientation.w = 0.5
right_arm.set_pose_target(start_pose)
plan_start = right_arm.plan()
print "Plan start"
rospy.sleep(5)
right_arm.execute(plan_start)
print "Execute start"
rospy.sleep(5)
end_pose = geometry_msgs.msg.Pose()
end_pose.position.x = -0.0434279649929
end_pose.position.y = -0.0562017053887
end_pose.position.z = 1.48763433664
end_pose.orientation.x = 0.5
end_pose.orientation.y = 0.5
end_pose.orientation.z = 0.5
end_pose.orientation.w = 0.5
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
rospy.Subscriber('/aruco_single/pose', PoseStamped, self.pose_cb,queue_size=1)
scene=PlanningSceneInterface()
self.scene_pub=rospy.Publisher('planning_scene',PlanningScene)
self.colors=dict()
rospy.sleep(1)
arm=MoveGroupCommander('arm')
#gripper=MoveGroupCommander('gripper')
end_effector_link=arm.get_end_effector_link()
arm.set_goal_position_tolerance(0.005)
arm.set_goal_orientation_tolerance(0.025)
arm.allow_replanning(True)
#gripper.set_goal_position_tolerance(0.005)
#gripper.set_goal_orientation_tolerance(0.025)
#gripper.allow_replanning(True)
reference_frame='base_link'
arm.set_pose_reference_frame(reference_frame)
arm.set_planning_time(5)
#scene planning
table_id='table'
#cylinder_id='cylinder'
box2_id='box2'
target_id='target_object'
#scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(table_id)
scene.remove_world_object(target_id)
rospy.sleep(2)
table_ground=0.59
table_size=[0.5,1,0.01]
#box1_size=[0.1,0.05,0.03]
box2_size=[0.15,0.15,0.02]
r_tool_size=[0.05,0.04,0.22]
l_tool_size=[0.05,0.04,0.22]
target_size=[0.05,0.05,0.1]
table_pose=PoseStamped()
table_pose.header.frame_id=reference_frame
table_pose.pose.position.x=0.7
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.7
box1_pose.pose.position.y=-0.2
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.55
box2_pose.pose.position.y=-0.12
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)
target_pose=PoseStamped()
target_pose.header.frame_id=reference_frame
target_pose.pose.position.x=0.58
target_pose.pose.position.y=0.05
target_pose.pose.position.z=table_ground+table_size[2]+target_size[2]/2.0
target_pose.pose.orientation.x=0
target_pose.pose.orientation.y=0
target_pose.pose.orientation.z=0
target_pose.pose.orientation.w=1
scene.add_box(target_id,target_pose,target_size)
#left gripper
l_p=PoseStamped()
l_p.header.frame_id=end_effector_link
l_p.pose.position.x=0.00
l_p.pose.position.y=0.06
l_p.pose.position.z=0.11
l_p.pose.orientation.w=1
scene.attach_box(end_effector_link,'l_tool',l_p,l_tool_size)
#right gripper
r_p=PoseStamped()
r_p.header.frame_id=end_effector_link
r_p.pose.position.x=0.00
r_p.pose.position.y= -0.06
r_p.pose.position.z=0.11
r_p.pose.orientation.w=1
scene.attach_box(end_effector_link,'r_tool',r_p,r_tool_size)
#grasp
g_p=PoseStamped()
g_p.header.frame_id=end_effector_link
g_p.pose.position.x=0.00
g_p.pose.position.y= -0.00
g_p.pose.position.z=0.025
g_p.pose.orientation.w=0.707
g_p.pose.orientation.x=0
g_p.pose.orientation.y=-0.707
g_p.pose.orientation.z=0
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)
self.setColor('r_tool',0.8,0,0)
self.setColor('l_tool',0.8,0,0)
self.setColor('target_object',0,1,0)
self.sendColors()
#motion planning
arm.set_named_target("initial_arm")
arm.go()
rospy.sleep(2)
grasp_pose=target_pose
grasp_pose.pose.position.x-=0.15
#grasp_pose.pose.position.z=
grasp_pose.pose.orientation.x=0
grasp_pose.pose.orientation.y=0.707
grasp_pose.pose.orientation.z=0
grasp_pose.pose.orientation.w=0.707
#arm.set_start_state_to_current_state()
'''
arm.set_pose_target(grasp_pose,end_effector_link)
traj=arm.plan()
arm.execute(traj)
print arm.get_current_joint_values()
'''
pre_joint_state=[0.16588150906995922, 1.7060146047438647, -0.00961761728757362, 1.8614674591892713, -2.9556667436476847, 1.7432451233907822, 3.1415]
arm.set_joint_value_target(pre_joint_state)
traj=arm.plan()
arm.execute(traj)
rospy.sleep(2)
arm.shift_pose_target(0,0.09,end_effector_link)
arm.go()
rospy.sleep(2)
scene.attach_box(end_effector_link,target_id,g_p,target_size)
rospy.sleep(2)
#grasping is over , from now is pouring
arm.shift_pose_target(2,0.15,end_effector_link)
arm.go()
rospy.sleep(2)
joint_state_1=arm.get_current_joint_values()
joint_state_1[0]-=0.17
arm.set_joint_value_target(joint_state_1)
arm.go()
rospy.sleep(1)
joint_state_2=arm.get_current_joint_values()
joint_state_2[6]-=1.8
arm.set_joint_value_target(joint_state_2)
arm.go()
rospy.sleep(1)
#print arm.get_current_joint_values()
#pouring test
for i in range(1,5):
joint_state_2[6]+=0.087
arm.set_joint_value_target(joint_state_2)
arm.go()
time.sleep(0.05)
joint_state_2[6]-=0.087
arm.set_joint_value_target(joint_state_2)
arm.go()
time.sleep(0.05)
print i
joint_state_2[6]+=1.8
arm.set_joint_value_target(joint_state_2)
arm.go()
rospy.sleep(2)
joint_state_1[0]+=0.17
arm.set_joint_value_target(joint_state_1)
arm.go()
rospy.sleep(1)
arm.shift_pose_target(2,-0.15,end_effector_link)
arm.go()
rospy.sleep(2)
scene.remove_attached_object(end_effector_link,target_id)
rospy.sleep(2)
arm.set_named_target("initial_arm")
arm.go()
rospy.sleep(2)
#remove and shut down
scene.remove_attached_object(end_effector_link,'l_tool')
rospy.sleep(1)
scene.remove_attached_object(end_effector_link,'r_tool')
rospy.sleep(1)
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
class SrFastGrasp:
def __init__(self):
self.__marker_pub = rospy.Publisher("visualization_marker",
Marker,
queue_size=1)
self.__grasp_server = rospy.Service("grasp_from_bounding_box",
GetFastGraspFromBoundingBox,
self.__bounding_box_cb)
self.__default_grasp = 'super_amazing_grasp'
self.__get_state = rospy.ServiceProxy(
'/grasp_warehouse/get_robot_state', GetState)
hand_group = rospy.get_param("~hand_group", "right_hand")
arm_group = rospy.get_param("~arm_group", "right_arm")
self.__group = MoveGroupCommander(hand_group)
self.__arm_g = MoveGroupCommander(arm_group)
self.__ik = rospy.ServiceProxy("compute_ik", GetPositionIK)
def __modify_grasp_pose(self, grasp, pose):
"""
Aligns grasp with axis from origin to center of object.
A crude way to make a vaguely sane orientation for the hand
that seems to more or less work.
"""
v1 = numpy.array(
[pose.pose.position.x, pose.pose.position.y, pose.pose.position.z])
v1_length = numpy.linalg.norm(v1)
v1 = v1 / v1_length
v2 = [1, 0, -v1[0] / v1[2]]
v2 = v2 / numpy.linalg.norm(v2)
v3 = numpy.cross(v1, v2)
v3 = v3 / numpy.linalg.norm(v3)
m = [[v3[0], v1[0], v2[0]], [v3[1], v1[1], v2[1]],
[v3[2], v1[2], v2[2]]]
q = quaternion_from_matrix(m)
grasp.grasp_pose = deepcopy(pose)
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]
def __bounding_box_cb(self, request):
box = request.bounding_box
pose = request.pose
if SolidPrimitive.BOX != box.type:
rospy.logerr("Bounding volume must be a BOX.")
return None
self.__send_marker_to_rviz(box, pose)
grasp_name = self.__select_grasp()
grasp = self.__get_grasp(grasp_name)
self.__modify_grasp_pose(grasp, pose)
return grasp
def __select_grasp(self):
return self.__default_grasp
def __get_grasp(self, name):
try:
open_state = self.__get_state(name + "_open", "").state
closed_state = self.__get_state(name + "_closed", "").state
except Exception:
rospy.logfatal("Couldn't get grasp pose from db.")
return Grasp()
try:
self.__group.set_start_state_to_current_state()
pre_pose = self.__group.plan(open_state.joint_state)
self.__group.set_start_state(open_state)
pose = self.__group.plan(closed_state.joint_state)
except Exception:
rospy.logfatal("Couldn't plan grasp trajectories.")
return Grasp()
grasp = Grasp()
grasp.id = name
grasp.pre_grasp_posture = pre_pose.joint_trajectory
grasp.grasp_posture = pose.joint_trajectory
grasp.pre_grasp_approach.desired_distance = 0.2
grasp.pre_grasp_approach.min_distance = 0.1
grasp.pre_grasp_approach.direction.vector.x = 0
grasp.pre_grasp_approach.direction.vector.y = -1
grasp.pre_grasp_approach.direction.vector.z = 0
return grasp
def __get_major_axis(self, box):
m = max(box.dimensions)
max_index = [i for i, j in enumerate(box.dimensions) if j == m]
return max_index[-1] # Get the LAST axis with max val.
def __send_marker_to_rviz(self, box, pose):
marker = self.__get_marker_from_box(box, pose)
self.__marker_pub.publish(marker)
def __get_marker_from_box(self, box, pose):
marker = Marker()
marker.pose = pose.pose
marker.header.frame_id = pose.header.frame_id
marker.scale.x = box.dimensions[SolidPrimitive.BOX_X]
marker.scale.y = box.dimensions[SolidPrimitive.BOX_Y]
marker.scale.z = box.dimensions[SolidPrimitive.BOX_Z]
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.color.a = 0.5
marker.lifetime = rospy.rostime.Duration()
marker.type = Marker.CUBE
marker.ns = "sr_fast_grasp_target"
marker.id = 0
marker.action = Marker.ADD
return marker
if __name__ == '__main__':
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
arm = MoveGroupCommander("manipulator")
eef = MoveGroupCommander("endeffector")
rospy.sleep(1)
#Start State Gripper
group_variable_values = eef.get_current_joint_values()
group_variable_values[0] = 0.0
eef.set_joint_value_target(group_variable_values)
plan2 = eef.plan()
arm.execute(plan2)
# clean the scene
scene.remove_world_object("pole")
scene.remove_world_object("table")
scene.remove_world_object("part")
# publish a demo scene
k = PoseStamped()
k.header.frame_id = robot.get_planning_frame()
k.pose.position.x = 0.0
k.pose.position.y = 0.0
k.pose.position.z = -0.05
scene.add_box("table", k, (2, 2, 0.0001))
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
#Initialize robot
robot = moveit_commander.RobotCommander()
# 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, queue_size=10)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10)
# Create a publisher for displaying object frames
self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10)
## Create a publisher for visualizing direction ###
self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the MoveIt! commander for the arm
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the MoveIt! commander for the gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Allow 5 seconds per planning attempt
right_arm.set_planning_time(5)
# Prepare Gripper and open it
self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction)
self.ac.wait_for_server()
g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100))
g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.0, 1))
self.ac.send_goal(g_open)
# Give the scene a chance to catch up
rospy.sleep(2)
# Prepare Gazebo Subscriber
self.pwh = None
self.pwh_copy = None
self.idx_targ = None
self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)
rospy.sleep(2)
# PREPARE THE SCENE
while self.pwh is None:
rospy.sleep(0.05)
target_id = 'target'
self.taid = self.pwh.name.index('wood_cube_5cm')
table_id = 'table'
self.tid = self.pwh.name.index('table')
#obstacle1_id = 'obstacle1'
#self.o1id = self.pwh.name.index('wood_block_10_2_1cm')
# Remove leftover objects from a previous run
scene.remove_world_object(target_id)
scene.remove_world_object(table_id)
#scene.remove_world_object(obstacle1_id)
# Remove any attached objects from a previous session
scene.remove_attached_object(GRIPPER_FRAME, target_id)
# Set the target size [l, w, h]
target_size = [0.05, 0.05, 0.05]
table_size = [1.5, 0.8, 0.03]
#obstacle1_size = [0.1, 0.025, 0.01]
## Set the target pose on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose = self.pwh.pose[self.taid]
target_pose.pose.position.z += 0.025
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose = self.pwh.pose[self.tid]
table_pose.pose.position.z += 1
scene.add_box(table_id, table_pose, table_size)
#obstacle1_pose = PoseStamped()
#obstacle1_pose.header.frame_id = REFERENCE_FRAME
#obstacle1_pose.pose = self.pwh.pose[self.o1id]
## Add the target object to the scene
#scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size)
# 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 = 0.50
place_pose.pose.position.y = -0.30
place_pose.pose.orientation.w = 1.0
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
### Make the target purple ###
self.setColor(target_id, 0.6, 0, 1, 1.0)
# Send the colors to the planning scene
self.sendColors()
#print target_pose
self.object_frames_pub.publish(target_pose)
rospy.sleep(2)
print "==================== Generating Transformations ==========================="
M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M1[0,3] = target_pose.pose.position.x
M1[1,3] = target_pose.pose.position.y
M1[2,3] = target_pose.pose.position.z
M2 = transformations.euler_matrix(0, 1.57, 0)
M2[0,3] = 0.0 # offset about x
M2[1,3] = 0.0 # about y
M2[2,3] = 0.25 # about z
T = np.dot(M1, M2)
pre_grasping = deepcopy(target_pose)
pre_grasping.pose.position.x = T[0,3]
pre_grasping.pose.position.y = T[1,3]
pre_grasping.pose.position.z = T[2,3]
quat = transformations.quaternion_from_matrix(T)
pre_grasping.pose.orientation.x = quat[0]
pre_grasping.pose.orientation.y = quat[1]
pre_grasping.pose.orientation.z = quat[2]
pre_grasping.pose.orientation.w = quat[3]
pre_grasping.header.frame_id = 'gazebo_world'
# # Initialize the grasp object
# g = Grasp()
# grasps = []
# # Set the first grasp pose to the input pose
# g.grasp_pose = pre_grasping
# g.allowed_touch_objects = [target_id]
# grasps.append(deepcopy(g))
# right_arm.pick(target_id, grasps)
#Change the frame_id for the planning to take place!
#target_pose.header.frame_id = 'gazebo_world'
self.p_pub.publish(pre_grasping)
right_arm.set_pose_target(pre_grasping, GRIPPER_FRAME)
plan = right_arm.plan()
rospy.sleep(2)
right_arm.go(wait=True)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
p5.header.frame_id = robot.get_planning_frame()
p5.pose.position.x = -0.6
p5.pose.position.y = 0.7
p5.pose.position.z = 0.1 - 0.025 + 0.105
scene.add_box("foor2", p5, (0.2, 0.4, 0.01))
rospy.sleep(1)
temPose = p
temPose.pose.position.z -= 0.01
temPose.pose.position.x += 0.125
temPose.pose.orientation.y = 1
# temPose.pose.orientation.w = 0.707
print temPose
arm.set_pose_target(temPose)
pl = arm.plan()
state = arm.execute(pl)
print state
rospy.sleep(1)
if (state):
cnt = -1
while cnt < 7:
client.wait_for_server()
goal = GripperCommandAction().action_goal.goal
if cnt < 0:
goal.command.position = 0.35
else:
goal.command.position = 0.445 - (2**(6 - cnt)) * 0.001
rospy.loginfo(goal.command.position)
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
# arm_group.set_goal_position_tolerance(0.001)
q = quaternion_from_euler(1.5701, 0.0, -1.5701)
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.x = q[0]
pose_target.orientation.y = q[1]
pose_target.orientation.z = q[2]
pose_target.orientation.w = q[3]
pose_target.position.z = 0.10 #0.25
pose_target.position.y = 0.00 #0.00
pose_target.position.x = -0.45 #-0.45
arm_group.set_pose_target(pose_target)
# # # ## Now, we call the planner to compute the plan and execute it.
plan = arm_group.plan(pose_target)
while plan[0] != True:
plan = arm_group.plan(pose_target)
if plan[0]:
traj = plan[1]
arm_group.execute(traj, wait=True)
arm_group.stop()
arm_group.clear_pose_targets()
# rospy.sleep(1)
#GO FRONT
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
scene = PlanningSceneInterface()
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
self.colors = dict()
rospy.sleep(1)
arm = MoveGroupCommander('arm')
end_effector_link = arm.get_end_effector_link()
arm.set_goal_position_tolerance(0.005)
arm.set_goal_orientation_tolerance(0.025)
arm.allow_replanning(True)
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
arm.set_planning_time(5)
#scene planning
table_id = 'table'
#cylinder_id='cylinder'
#box1_id='box1'
box2_id = 'box2'
target_id = 'target_object'
#scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(table_id)
scene.remove_world_object(target_id)
rospy.sleep(2)
table_ground = 0.68
table_size = [0.5, 1, 0.01]
#box1_size=[0.1,0.05,0.03]
box2_size = [0.05, 0.05, 0.1]
r_tool_size = [0.03, 0.01, 0.06]
l_tool_size = [0.03, 0.01, 0.06]
target_size = [0.05, 0.05, 0.1]
table_pose = PoseStamped()
table_pose.header.frame_id = reference_frame
table_pose.pose.position.x = 0.75
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.7
box1_pose.pose.position.y=-0.2
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.6
box2_pose.pose.position.y = -0.05
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)
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.pose.position.x = 0.6
target_pose.pose.position.y = 0.05
target_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
target_pose.pose.orientation.x = 0
target_pose.pose.orientation.y = 0
target_pose.pose.orientation.z = 0
target_pose.pose.orientation.w = 1
scene.add_box(target_id, target_pose, target_size)
#left gripper
l_p = PoseStamped()
l_p.header.frame_id = end_effector_link
l_p.pose.position.x = 0.00
l_p.pose.position.y = 0.04
l_p.pose.position.z = 0.04
l_p.pose.orientation.w = 1
scene.attach_box(end_effector_link, 'l_tool', l_p, l_tool_size)
#right gripper
r_p = PoseStamped()
r_p.header.frame_id = end_effector_link
r_p.pose.position.x = 0.00
r_p.pose.position.y = -0.04
r_p.pose.position.z = 0.04
r_p.pose.orientation.w = 1
scene.attach_box(end_effector_link, 'r_tool', r_p, r_tool_size)
#grasp
g_p = PoseStamped()
g_p.header.frame_id = end_effector_link
g_p.pose.position.x = 0.00
g_p.pose.position.y = -0.00
g_p.pose.position.z = 0.025
g_p.pose.orientation.w = 0.707
g_p.pose.orientation.x = 0
g_p.pose.orientation.y = -0.707
g_p.pose.orientation.z = 0
#set color
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)
self.setColor('r_tool', 0.8, 0, 0)
self.setColor('l_tool', 0.8, 0, 0)
self.setColor('target_object', 0, 1, 0)
self.sendColors()
#motion planning
arm.set_named_target("initial_arm1")
arm.go()
rospy.sleep(2)
grasp_pose = target_pose
grasp_pose.pose.position.x -= 0.15
#grasp_pose.pose.position.z=
grasp_pose.pose.orientation.x = 0
grasp_pose.pose.orientation.y = 0.707
grasp_pose.pose.orientation.z = 0
grasp_pose.pose.orientation.w = 0.707
arm.set_start_state_to_current_state()
arm.set_pose_target(grasp_pose, end_effector_link)
traj = arm.plan()
arm.execute(traj)
rospy.sleep(2)
print arm.get_current_joint_values()
#arm.shift_pose_target(4,1.57,end_effector_link)
#arm.go()
#rospy.sleep(2)
arm.shift_pose_target(0, 0.11, end_effector_link)
arm.go()
rospy.sleep(2)
print arm.get_current_joint_values()
saved_target_pose = arm.get_current_pose(end_effector_link)
#arm.set_named_target("initial_arm2")
#grasp
scene.attach_box(end_effector_link, target_id, g_p, target_size)
rospy.sleep(2)
#grasping is over , from now is placing
arm.shift_pose_target(2, 0.15, end_effector_link)
arm.go()
rospy.sleep(2)
print arm.get_current_joint_values()
arm.shift_pose_target(1, -0.2, end_effector_link)
arm.go()
rospy.sleep(2)
print arm.get_current_joint_values()
arm.shift_pose_target(2, -0.15, end_effector_link)
arm.go()
rospy.sleep(2)
print arm.get_current_joint_values()
scene.remove_attached_object(end_effector_link, target_id)
rospy.sleep(2)
#arm.set_pose_target(saved_target_pose,end_effector_link)
#arm.go()
#rospy.sleep(2)
arm.set_named_target("initial_arm1")
arm.go()
rospy.sleep(2)
#remove and shut down
scene.remove_attached_object(end_effector_link, 'l_tool')
scene.remove_attached_object(end_effector_link, 'r_tool')
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
class BaxterMoveit(BaxterRobot):
LEFT = -1
RIGHT = 1
def __init__(self, node, limb=+1):
super(BaxterMoveit, self).__init__(node, limb=+1)
self.group = MoveGroupCommander(self.lns + "_arm")
self.robot = RobotCommander()
self.scene = PlanningSceneInterface()
self._info()
def _info(self):
'''Getting Basic Information'''
# name of the reference frame for this robot:
print("@@@@@@@@@@@@ Reference frame: %s" %
self.group.get_planning_frame())
# We can also print the name of the end-effector link for this group:
print("@@@@@@@@@@@@ End effector: %s" %
self.group.get_end_effector_link())
# We can get a list of all the groups in the robot:
print("@@@@@@@@@@@@ Robot Groups: @@@@@@@@@@@@@",
self.robot.get_group_names())
# Sometimes for debugging it is useful to print the entire state of the
# robot:
print("@@@@@@@@@@@@ Printing robot state @@@@@@@@@@@@@")
print(self.robot.get_current_state())
def obstacle(self, name, position, size):
planning_frame = self.robot.get_planning_frame()
pose = PoseStamped()
pose.header.frame_id = planning_frame
pose.pose.position.x = position[0]
pose.pose.position.y = position[1]
pose.pose.position.z = position[2]
self.scene.add_box(name, pose, tuple(size))
def movej(self, q, raw=False):
''' move in joint space by giving a joint configuration as dictionary'''
if raw:
print("in moveit 'raw' motion is not avaiable")
# succ = False
# while succ is False:
succ = self.group.go(q, wait=True)
# self.group.stop() # ensures that there is no residual movement
def showplan(self, target):
if type(target) is PyKDL.Frame or type(target) is Pose:
q = self.ik(target)
elif type(target) is dict:
q = target
else:
print("Target format error")
return
self.group.set_joint_value_target(q)
self.group.plan()
def movep(self, pose, raw=False):
''' move the eef in Cartesian space by giving a geometry_msgs.Pose or a PyKDL.Frame'''
if type(pose) is PyKDL.Frame:
pose = transformations.KDLToPose(pose)
q = self.ik(pose)
if q is not None:
self.movej(q, raw=raw)
else:
print("\nNO SOLUTION TO IK\n" * 20)
'/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory)
#Call the planner to compute the plan and visualize it if successful.
print "============ Generating plan for left arm"
pose_target_left = geometry_msgs.msg.Pose()
pose_target_left.orientation.x = 0.69283
pose_target_left.orientation.y = 0.1977
pose_target_left.orientation.z = -0.16912
pose_target_left.orientation.w = 0.67253
pose_target_left.position.x = 0.81576
pose_target_left.position.y = 0.093893
pose_target_left.position.z = 0.2496
group_left_arm.set_pose_target(pose_target_left)
#group_left_arm.set_position_target([0.75,0.27,0.35])
plan_1eft = group_left_arm.plan()
#print "Trajectory time (nsec): ", plan_left.joint_trajectory.points[len(plan_left.joint_trajectory.points)-1].time_from_start
rospy.sleep(5)
print "============ Generating plan for right arm"
pose_target_right = geometry_msgs.msg.Pose()
pose_target_right.orientation.x = 0.56508
pose_target_right.orientation.y = -0.5198
pose_target_right.orientation.z = -0.54332
pose_target_right.orientation.w = -0.33955
pose_target_right.position.x = 0.72651
pose_target_right.position.y = -0.041037
pose_target_right.position.z = 0.19097
group_right_arm.set_pose_target(pose_target_right)
#group_right_arm.set_position_target([0.75,-0.27,0.35])
