def _home_arm_planner(self):
if self._prefix == 'left':
rospy.sleep(5)
else:
move_group_jtas = MoveGroupInterface("upper_body", "base_link")
move_group_jtas.setPlannerId("RRTConnectkConfigDefault")
success = False
while not rospy.is_shutdown() and not success:
result = move_group_jtas.moveToJointPosition(
self._body_joints, self._homed, 0.05)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.logerr("_home_arm_planner completed ")
success = True
else:
rospy.logerr(
"_home_arm_planner: _home_arm_planner failed (%d)" %
result.error_code.val)
self._arms_homing = True
self._ctl.api.MoveHome()
self._ctl.api.InitFingers()
self.home_arm_pub.publish(Bool(True))
rospy.sleep(2.0)
self._arms_homing = False
self._planner_homing = False
class MovoUpperBody:
upper_body_joints = [
"right_shoulder_pan_joint", "right_shoulder_lift_joint",
"right_arm_half_joint", "right_elbow_joint",
"right_wrist_spherical_1_joint", "right_wrist_spherical_2_joint",
"right_wrist_3_joint", "left_shoulder_pan_joint",
"left_shoulder_lift_joint", "left_arm_half_joint", "left_elbow_joint",
"left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint",
"left_wrist_3_joint", "linear_joint", "pan_joint", "tilt_joint"
]
homed = [
-1.5,-0.2,-0.175,-2.0,2.0,-1.24,-1.1, \
1.5,0.2,0.15,2.0,-2.0,1.24,1.1,\
0.35,0,0
]
tucked = [
-1.6,-1.4,0.4,-2.7,0.0,0.5,-1.7,\
1.6,1.4,-0.4,2.7,0.0,-0.5, 1.7,
0.04, 0, 0]
sidearms = [
-0.3 , -1.7 ,0.0 ,-1.7 ,1.8 ,-1.8 ,-1.1, \
0.3 , 1.7 ,0.0 ,1.7 ,-1.8 ,1.8 ,1.1 ,\
.35, 0, -1]
gripper_closed = 0.01
gripper_open = 0.165
def __init__(self):
self.move_group = MoveGroupInterface("upper_body", "base_link")
self.move_group.setPlannerId("RRTConnectkConfigDefault")
self.lgripper = GripperActionClient('left')
self.rgripper = GripperActionClient('right')
def move(self, pose):
success = False
while not rospy.is_shutdown() and not success:
result = self.move_group.moveToJointPosition(\
self.upper_body_joints, pose, 0.05)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
success = True
else:
rospy.logerr("moveToJointPosition failed (%d)"\
%result.error_code.val)
def untuck(self):
rospy.loginfo("untucking the arms")
self.lgripper.command(self.gripper_open)
self.rgripper.command(self.gripper_open)
self.move(self.homed)
def tuck(self):
self.move(self.tucked)
self.lgripper.command(self.gripper_closed)
self.rgripper.command(self.gripper_closed)
def side(self):
self.move(self.sidearms)
class GraspingClient(object):
def __init__(self, sim=False):
# Define planning groups
self.scene = PlanningSceneInterface("base_link")
self.dof = rospy.get_param('~jaco_dof')
self.move_group = MoveGroupInterface("upper_body", "base_link")
self.rmove_group = MoveGroupInterface("right_arm", "base_link")
planner_id = "RRTConnectkConfigDefault"
self.move_group.setPlannerId(planner_id)
self.rmove_group.setPlannerId(planner_id)
self.objects_heights = [0.122, 0.240]
self.objects_heights_tolerances = [0.02, 0.03]
# Define joints and positions for 6 and 7 DOF
if "6dof" == self.dof:
self._upper_body_joints = [
"right_elbow_joint", "right_shoulder_lift_joint",
"right_shoulder_pan_joint", "right_wrist_1_joint",
"right_wrist_2_joint", "right_wrist_3_joint",
"left_elbow_joint", "left_shoulder_lift_joint",
"left_shoulder_pan_joint", "left_wrist_1_joint",
"left_wrist_2_joint", "left_wrist_3_joint", "linear_joint",
"pan_joint", "tilt_joint"
]
self._right_arm_joints = [
"right_elbow_joint", "right_shoulder_lift_joint",
"right_shoulder_pan_joint", "right_wrist_1_joint",
"right_wrist_2_joint", "right_wrist_3_joint"
]
self._left_arm_joints = [
"left_elbow_joint", "left_shoulder_lift_joint",
"left_shoulder_pan_joint", "left_wrist_1_joint",
"left_wrist_2_joint", "left_wrist_3_joint"
]
self.tucked = [
-2.8, -1.48, -1.48, 0, 0, 1.571, 2.8, 1.48, 1.48, 0, 0, -1.571,
0.0371, 0.0, 0.0
]
self.constrained_stow = [
2.28, 2.00, -2.56, -0.09, 0.15, 1.082, -2.28, -2.17, 2.56,
0.09, -0.15, 2.06, 0.42, 0.0, 0.0
]
self.rarm_const_stow = [2.28, 2.00, -2.56, -0.09, 0.15, 1.06]
#self.rarm_const_stow = [-1.51, -0.22, -2.00, -2.04, 1.42, 1.32]
elif "7dof" == self.dof:
self._upper_body_joints = [
"right_shoulder_pan_joint", "right_shoulder_lift_joint",
"right_arm_half_joint", "right_elbow_joint",
"right_wrist_spherical_1_joint",
"right_wrist_spherical_2_joint", "right_wrist_3_joint",
"left_shoulder_pan_joint", "left_shoulder_lift_joint",
"left_arm_half_joint", "left_elbow_joint",
"left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint",
"left_wrist_3_joint", "linear_joint", "pan_joint", "tilt_joint"
]
self._right_arm_joints = [
"right_shoulder_pan_joint", "right_shoulder_lift_joint",
"right_arm_half_joint", "right_elbow_joint",
"right_wrist_spherical_1_joint",
"right_wrist_spherical_2_joint", "right_wrist_3_joint"
]
self._left_arm_joints = [
"left_shoulder_pan_joint", "left_shoulder_lift_joint",
"left_arm_half_joint", "left_elbow_joint",
"left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint",
"left_wrist_3_joint"
]
self.tucked = [
-1.6, -1.5, 0.4, -2.7, 0.0, 0.5, -1.7, 1.6, 1.5, -0.4, 2.7,
0.0, -0.5, 1.7, 0.04, 0, 0
]
self.constrained_stow = [
-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, 1.0, 2.6, -2.0, 0.0, -2.0, 0.0,
0.0, -1.0, 0.42, 0, 0
]
self.rarm_const_stow = [-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, -1.0]
else:
rospy.logerr("DoF needs to be set 6 or 7, aborting demo")
return
# Define the MoveIt pickplace pipeline
self.pickplace = None
self.pickplace = PickPlaceInterface("left_side",
"left_gripper",
verbose=True)
self.pickplace.planner_id = planner_id
self.pick_result = None
self.place_result = None
self._listener = tf.TransformListener()
self._lgripper = GripperActionClient('left')
find_grasp_planning_topic = "grasp_planner/plan"
rospy.loginfo("Waiting for %s..." % find_grasp_planning_topic)
self.find_grasp_planning_client = actionlib.SimpleActionClient(
find_grasp_planning_topic, GraspPlanningAction)
self.find_grasp_planning_client.wait_for_server()
rospy.loginfo("...connected")
self.scene.clear()
# This is a simulation so need to adjust gripper parameters
if sim:
self._gripper_closed = 0.96
self._gripper_open = 0.00
else:
self._gripper_closed = 0.01
self._gripper_open = 0.165
def __del__(self):
self.scene.clear()
def add_objects_to_keep(self, obj):
self._objs_to_keep.append(obj)
def clearScene(self):
self.scene.clear()
def calculateGraspForObject(self, object_to_grasp, gripper):
goal = GraspPlanningGoal()
goal.object = object_to_grasp
goal.gripper = gripper
self.find_grasp_planning_client.send_goal(goal)
self.find_grasp_planning_client.wait_for_result(rospy.Duration(5.0))
return self.find_grasp_planning_client.get_result(
).grasps #moveit_msgs/Grasp[]
def pick(self, block, grasps):
success, pick_result = self.pickplace.pick_with_retry(block.name,
grasps,
retries=1,
scene=self.scene)
self.pick_result = pick_result
return success
def place(self, block, pose_stamped):
places = list()
l = PlaceLocation()
l.place_pose.pose = pose_stamped.pose
l.place_pose.header.frame_id = pose_stamped.header.frame_id
# copy the posture, approach and retreat from the grasp used
l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach
l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
places.append(copy.deepcopy(l))
# create another several places, rotate each by 360/m degrees in yaw direction
m = 16 # number of possible place poses
pi = 3.141592653589
for i in range(0, m - 1):
l.place_pose.pose = rotate_pose_msg_by_euler_angles(
l.place_pose.pose, 0, 0, 2 * pi / m)
places.append(copy.deepcopy(l))
success, place_result = self.pickplace.place_with_retry(
block.name, places, retries=1, scene=self.scene)
self.place_result = place_result
return success
def goto_tuck(self):
# remove previous objects
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(
self._upper_body_joints, self.tucked, 0.05)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def goto_plan_grasp(self):
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(
self._upper_body_joints, self.constrained_stow, 0.05)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def left_arm_constrained_stow(self):
c1 = Constraints()
c1.orientation_constraints.append(OrientationConstraint())
c1.orientation_constraints[0].header.stamp = rospy.get_rostime()
c1.orientation_constraints[0].header.frame_id = "base_link"
c1.orientation_constraints[0].link_name = "left_ee_link"
c1.orientation_constraints[0].orientation.w = 1.0
c1.orientation_constraints[
0].absolute_x_axis_tolerance = 0.2 #x axis is pointed up for wrist link
c1.orientation_constraints[0].absolute_y_axis_tolerance = 0.2
c1.orientation_constraints[0].absolute_z_axis_tolerance = 6.28
c1.orientation_constraints[0].weight = 1.0
while not rospy.is_shutdown():
result = self.lmove_group.moveToJointPosition(
self._left_arm_joints,
self.larm_const_stow,
0.05,
path_constraints=c1,
planning_time=120.0)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def right_arm_constrained_stow(self):
# c1 = Constraints()
# c1.orientation_constraints.append(OrientationConstraint())
# c1.orientation_constraints[0].header.stamp = rospy.get_rostime()
# c1.orientation_constraints[0].header.frame_id = "base_link"
# c1.orientation_constraints[0].link_name = "right_ee_link"
# c1.orientation_constraints[0].orientation.w=1.0
# c1.orientation_constraints[0].absolute_x_axis_tolerance = 0.2 #x axis is pointed up for wrist link
# c1.orientation_constraints[0].absolute_y_axis_tolerance = 0.2
# c1.orientation_constraints[0].absolute_z_axis_tolerance = 6.28
# c1.orientation_constraints[0].weight = 1.0
while not rospy.is_shutdown():
result = self.rmove_group.moveToJointPosition(
self._right_arm_joints,
self.rarm_const_stow,
0.05,
planning_time=120.0)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def open_gripper(self):
self._lgripper.command(self._gripper_open, block=True)
def close_gripper(self):
self._lgripper.command(self._gripper_closed, block=True)
rospy.init_node('follow_me_pose')
voice_pub = rospy.Publisher("/movo/voice/text",
String,
queue_size=1,
latch=True)
voice_cmd = String()
voice_cmd.data = "Do you want to dance with me?"
# voice_cmd.data = "Long fei, stop wasting time on me. It will not work, unless you say I like movo."
voice_pub.publish(voice_cmd)
dof = rospy.get_param('~jaco_dof')
rmove_group = MoveGroupInterface("right_arm", "base_link")
lmove_group = MoveGroupInterface("left_arm", "base_link")
lmove_group.setPlannerId("RRTConnectkConfigDefault")
rmove_group.setPlannerId("RRTConnectkConfigDefault")
lgripper = GripperActionClient('left')
rgripper = GripperActionClient('right')
if "6dof" == dof:
right_arm_joints = [
"right_shoulder_pan_joint", "right_shoulder_lift_joint",
"right_elbow_joint", "right_wrist_1_joint", "right_wrist_2_joint",
"right_wrist_3_joint"
]
left_arm_joints = [
"left_shoulder_pan_joint", "left_shoulder_lift_joint",
"left_elbow_joint", "left_wrist_1_joint", "left_wrist_2_joint",
"left_wrist_3_joint"
]
class GraspingClient(object):
def __init__(self, sim=False):
self.scene = PlanningSceneInterface("base_link")
self.dof = rospy.get_param('~jaco_dof')
self.move_group = MoveGroupInterface("upper_body", "base_link")
self.lmove_group = MoveGroupInterface("left_arm", "base_link")
self.rmove_group = MoveGroupInterface("right_arm", "base_link")
self.move_group.setPlannerId("RRTConnectkConfigDefault")
self.lmove_group.setPlannerId("RRTConnectkConfigDefault")
self.rmove_group.setPlannerId("RRTConnectkConfigDefault")
if "6dof" == self.dof:
self._upper_body_joints = [
"right_elbow_joint", "right_shoulder_lift_joint",
"right_shoulder_pan_joint", "right_wrist_1_joint",
"right_wrist_2_joint", "right_wrist_3_joint",
"left_elbow_joint", "left_shoulder_lift_joint",
"left_shoulder_pan_joint", "left_wrist_1_joint",
"left_wrist_2_joint", "left_wrist_3_joint", "linear_joint",
"pan_joint", "tilt_joint"
]
self._right_arm_joints = [
"right_elbow_joint", "right_shoulder_lift_joint",
"right_shoulder_pan_joint", "right_wrist_1_joint",
"right_wrist_2_joint", "right_wrist_3_joint"
]
self._left_arm_joints = [
"left_elbow_joint", "left_shoulder_lift_joint",
"left_shoulder_pan_joint", "left_wrist_1_joint",
"left_wrist_2_joint", "left_wrist_3_joint"
]
self.tucked = [
-2.8, -1.48, -1.48, 0, 0, 1.571, 2.8, 1.48, 1.48, 0, 0, -1.571,
0.0371, 0.0, 0.0
]
self.constrained_stow = [
2.28, 2.17, -2.56, -0.09, 0.15, 1.082, -2.28, -2.17, 2.56,
0.09, -0.15, 2.06, 0.42, 0.0, 0.0
]
self.larm_const_stow = [-2.28, -2.17, 2.56, 0.09, -0.15, 2.08]
self.rarm_const_stow = [2.28, 2.17, -2.56, -0.09, 0.15, 1.06]
self.tableDist = 0.7
elif "7dof" == self.dof:
self._upper_body_joints = [
"right_shoulder_pan_joint", "right_shoulder_lift_joint",
"right_arm_half_joint", "right_elbow_joint",
"right_wrist_spherical_1_joint",
"right_wrist_spherical_2_joint", "right_wrist_3_joint",
"left_shoulder_pan_joint", "left_shoulder_lift_joint",
"left_arm_half_joint", "left_elbow_joint",
"left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint",
"left_wrist_3_joint", "linear_joint", "pan_joint", "tilt_joint"
]
self._right_arm_joints = [
"right_shoulder_pan_joint", "right_shoulder_lift_joint",
"right_arm_half_joint", "right_elbow_joint",
"right_wrist_spherical_1_joint",
"right_wrist_spherical_2_joint", "right_wrist_3_joint"
]
self._left_arm_joints = [
"left_shoulder_pan_joint", "left_shoulder_lift_joint",
"left_arm_half_joint", "left_elbow_joint",
"left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint",
"left_wrist_3_joint"
]
self.tucked = [
-1.6, -1.5, 0.4, -2.7, 0.0, 0.5, -1.7, 1.6, 1.5, -0.4, 2.7,
0.0, -0.5, 1.7, 0.04, 0, 0
]
self.constrained_stow = [
-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, 1.0, 2.6, -2.0, 0.0, -2.0, 0.0,
0.0, -1.0, 0.42, 0, 0
]
self.larm_const_stow = [2.6, -2.0, 0.0, -2.0, 0.0, 0.0, 1.0]
self.rarm_const_stow = [-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, -1.0]
self.tableDist = 0.8
else:
rospy.logerr("DoF needs to be set 6 or 7, aborting demo")
return
self.pickplace = [None] * 2
self.pickplace[0] = PickPlaceInterface("left_side",
"left_gripper",
verbose=True)
self.pickplace[0].planner_id = "RRTConnectkConfigDefault"
self.pickplace[1] = PickPlaceInterface("right_side",
"right_gripper",
verbose=True)
self.pickplace[1].planner_id = "RRTConnectkConfigDefault"
self.pick_result = [None] * 2
self._last_gripper_picked = 0
self.place_result = [None] * 2
self._last_gripper_placed = 0
self._objs_to_keep = []
self._listener = tf.TransformListener()
self._lgripper = GripperActionClient('left')
self._rgripper = GripperActionClient('right')
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(
find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
self.scene.clear()
# This is a simulation so need to adjust gripper parameters
if sim:
self._gripper_closed = 0.96
self._gripper_open = 0.00
else:
self._gripper_closed = 0.01
self._gripper_open = 0.165
def add_objects_to_keep(self, obj):
self._objs_to_keep.append(obj)
def clearScene(self):
self.scene.clear()
def getPickCoordinates(self):
self.updateScene(0, False)
beer, grasps = self.getGraspableBeer(False)
pringles, grasps = self.getGraspablePringles(False)
if (None == beer) or (None == pringles):
return None
center_objects = (beer.primitive_poses[0].position.y +
pringles.primitive_poses[0].position.y) / 2
surface = self.getSupportSurface(beer.support_surface)
tmp1 = surface.primitive_poses[
0].position.x - surface.primitives[0].dimensions[0] / 2
surface = self.getSupportSurface(pringles.support_surface)
tmp2 = surface.primitive_poses[
0].position.x - surface.primitives[0].dimensions[0] / 2
front_edge = (tmp1 + tmp2) / 2
coords = Pose2D(x=(front_edge - self.tableDist),
y=center_objects,
theta=0.0)
return coords
def updateScene(self, gripper=0, plan=True):
# find objects
rospy.loginfo("Updating scene...")
goal = FindGraspableObjectsGoal()
goal.plan_grasps = plan
goal.gripper = gripper
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
find_result = self.find_client.get_result()
# remove previous objects
for name in self.scene.getKnownCollisionObjects():
self.scene.removeCollisionObject(name, True)
# insert objects to scene
idx = -1
for obj in find_result.objects:
if obj.object.primitive_poses[
0].position.z < 0.5 or obj.object.primitive_poses[
0].position.x > 2.0 or obj.object.primitive_poses[
0].position.y > 0.5:
continue
idx += 1
obj.object.name = "object%d_%d" % (idx, gripper)
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
wait=True)
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
if obj.primitive_poses[0].position.z < 0.5:
continue
height = obj.primitive_poses[0].position.z
obj.primitives[0].dimensions = [
obj.primitives[0].dimensions[0],
obj.primitives[0].dimensions[1], # wider
obj.primitives[0].dimensions[2] + height
]
obj.primitive_poses[0].position.z += -height / 2.0
# add to scene
self.scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
wait=True)
self.scene.waitForSync()
# store for grasping
self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
def getGraspableBeer(self, planned=True):
for obj in self.objects:
# need grasps
if len(obj.grasps) < 1 and planned:
continue
# has to be on table
if obj.object.primitive_poses[0].position.z < 0.5 or \
obj.object.primitive_poses[0].position.z > 1.0 or \
obj.object.primitive_poses[0].position.x > 2.0:
continue
elif (obj.object.primitives[0].type == sp.CYLINDER):
if obj.object.primitives[0].dimensions[sp.CYLINDER_HEIGHT] < 0.102 or \
obj.object.primitives[0].dimensions[sp.CYLINDER_HEIGHT] > 0.142:
continue
elif (obj.object.primitives[0].type == sp.BOX):
if obj.object.primitives[0].dimensions[sp.BOX_Z] < 0.102 or \
obj.object.primitives[0].dimensions[sp.BOX_Z] > 0.142:
continue
else:
continue
print "beer: ", obj.object.primitive_poses[0]
return obj.object, obj.grasps
# nothing detected
return None, None
def getGraspablePringles(self, planned=True):
for obj in self.objects:
# need grasps
if len(obj.grasps) < 1 and planned:
continue
# has to be on table
if obj.object.primitive_poses[0].position.z < 0.5 or \
obj.object.primitive_poses[0].position.z > 1.0 or \
obj.object.primitive_poses[0].position.x > 2.0:
continue
elif (obj.object.primitives[0].type == sp.CYLINDER):
if obj.object.primitives[0].dimensions[sp.CYLINDER_HEIGHT] < 0.21 or \
obj.object.primitives[0].dimensions[sp.CYLINDER_HEIGHT] > 0.28:
continue
elif (obj.object.primitives[0].type == sp.BOX):
if obj.object.primitives[0].dimensions[sp.BOX_Z] < 0.21 or \
obj.object.primitives[0].dimensions[sp.BOX_Z] > 0.28:
continue
else:
continue
return obj.object, obj.grasps
# nothing detected
return None, None
def getSupportSurface(self, name):
for surface in self.surfaces:
if surface.name == name:
return surface
return None
def getPlaceLocation(self):
pass
def pick(self, block, grasps, gripper=0):
success, pick_result = self.pickplace[gripper].pick_with_retry(
block.name,
grasps,
retries=1,
support_name=block.support_surface,
scene=self.scene)
self.pick_result[gripper] = pick_result
self._last_gripper_picked = gripper
return success
def place(self, block, pose_stamped, gripper=0):
places = list()
l = PlaceLocation()
l.place_pose.pose = pose_stamped.pose
l.place_pose.header.frame_id = pose_stamped.header.frame_id
# copy the posture, approach and retreat from the grasp used
l.post_place_posture = self.pick_result[
gripper].grasp.pre_grasp_posture
l.pre_place_approach = self.pick_result[
gripper].grasp.pre_grasp_approach
l.post_place_retreat = self.pick_result[
gripper].grasp.post_grasp_retreat
places.append(copy.deepcopy(l))
# create another several places, rotate each by 360/m degrees in yaw direction
m = 16 # number of possible place poses
pi = 3.141592653589
for i in range(0, m - 1):
l.place_pose.pose = rotate_pose_msg_by_euler_angles(
l.place_pose.pose, 0, 0, 2 * pi / m)
places.append(copy.deepcopy(l))
success, place_result = self.pickplace[gripper].place_with_retry(
block.name, places, retries=1, scene=self.scene)
self.place_result[gripper] = place_result
self._last_gripper_placed = gripper
return success
def goto_tuck(self):
# remove previous objects
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(
self._upper_body_joints, self.tucked, 0.05)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def goto_plan_grasp(self):
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(
self._upper_body_joints, self.constrained_stow, 0.05)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def left_arm_constrained_stow(self):
c1 = Constraints()
c1.orientation_constraints.append(OrientationConstraint())
c1.orientation_constraints[0].header.stamp = rospy.get_rostime()
c1.orientation_constraints[0].header.frame_id = "base_link"
c1.orientation_constraints[0].link_name = "left_ee_link"
c1.orientation_constraints[0].orientation.w = 1.0
c1.orientation_constraints[
0].absolute_x_axis_tolerance = 0.2 #x axis is pointed up for wrist link
c1.orientation_constraints[0].absolute_y_axis_tolerance = 0.2
c1.orientation_constraints[0].absolute_z_axis_tolerance = 6.28
c1.orientation_constraints[0].weight = 1.0
while not rospy.is_shutdown():
result = self.lmove_group.moveToJointPosition(
self._left_arm_joints,
self.larm_const_stow,
0.05,
path_constraints=c1,
planning_time=120.0)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def right_arm_constrained_stow(self):
c1 = Constraints()
c1.orientation_constraints.append(OrientationConstraint())
c1.orientation_constraints[0].header.stamp = rospy.get_rostime()
c1.orientation_constraints[0].header.frame_id = "base_link"
c1.orientation_constraints[0].link_name = "right_ee_link"
c1.orientation_constraints[0].orientation.w = 1.0
c1.orientation_constraints[
0].absolute_x_axis_tolerance = 0.2 #x axis is pointed up for wrist link
c1.orientation_constraints[0].absolute_y_axis_tolerance = 0.2
c1.orientation_constraints[0].absolute_z_axis_tolerance = 6.28
c1.orientation_constraints[0].weight = 1.0
while not rospy.is_shutdown():
result = self.rmove_group.moveToJointPosition(
self._right_arm_joints,
self.rarm_const_stow,
0.05,
path_constraints=c1,
planning_time=120.0)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def open_gripper(self):
self._lgripper.command(self._gripper_open, block=True)
self._rgripper.command(self._gripper_open, block=True)
def close_gripper(self):
self._lgripper.command(self._gripper_closed, block=True)
self._rgripper.command(self._gripper_closed, block=True)
def main():
rospy.init_node('injerrobot_operation')
io_mod = io_module.IoModule(sim=True)
rootstock_params = rospy.get_param(
'/rootstock') ### XXX: this must be on userdata??
path_constraints = None
rootstock_arm_move_group = MoveGroupInterface(
"left_arm",
fixed_frame="left_arm_base_link",
gripper_frame="left_arm_link_6")
rootstock_arm_move_group.setPlannerId('RRTConnectkConfigDefault')
rootstock_arm_move_group.setPathConstraints(path_constraints)
rootstock_goto_init = GoTo(sim=True)
rootstock_goto_init.move_group = rootstock_arm_move_group
rootstock_goto_init.params = rootstock_params['init']
rootstock_goto_init.label = 'rootstock'
rootstock_goto_pick_feeder = GoToGrid(sim=True)
rootstock_goto_pick_feeder.move_group = rootstock_arm_move_group
rootstock_goto_pick_feeder.params = rootstock_params[
'feeder'] ### XXX: name: feeder???
rootstock_goto_pick_feeder.label = 'rootstock'
rootstock_goto_cut = GoTo(sim=True)
rootstock_goto_cut.move_group = rootstock_arm_move_group
rootstock_goto_cut.params = rootstock_params['cut']
rootstock_goto_cut.label = 'rootstock'
rootstock_goto_place_clip = GoTo(sim=True)
rootstock_goto_place_clip.move_group = rootstock_arm_move_group
rootstock_goto_place_clip.params = rootstock_params['clip']
rootstock_goto_place_clip.label = 'rootstock'
rootstock_goto_dispense = GoTo(sim=True)
rootstock_goto_dispense.move_group = rootstock_arm_move_group
rootstock_goto_dispense.params = rootstock_params['dispense']
rootstock_goto_dispense.label = 'rootstock'
rootstock_goto_inspection = GoTo(sim=True)
rootstock_goto_inspection.move_group = rootstock_arm_move_group
rootstock_goto_inspection.params = rootstock_params['inspection']
rootstock_goto_inspection.label = 'rootstock'
rootstock_goto_reject = GoTo(sim=True)
rootstock_goto_reject.move_group = rootstock_arm_move_group
rootstock_goto_reject.params = rootstock_params['reject']
rootstock_goto_reject.label = 'rootstock'
rootstock_pick = Pick(sim=True)
rootstock_pick.io_module = io_mod
rootstock_pick.label = 'rootstock'
rootstock_cut = Cut(sim=True)
rootstock_cut.io_module = io_mod
rootstock_cut.label = 'rootstock'
rootstock_place = Place(sim=True)
rootstock_place.io_module = io_mod
rootstock_place.label = 'rootstock'
rootstock_clip = Clip(sim=True)
rootstock_clip.io_module = io_mod
rootstock_clip.label = 'rootstock'
rootstock_inspection = Inspection(sim=True)
rootstock_inspection.io_module = io_mod
rootstock_inspection.label = 'rootstock'
rootstock_reject = Reject(sim=True)
rootstock_reject.io_module = io_mod
rootstock_reject.label = 'rootstock'
rootstock_dispense = Dispense(sim=True)
rootstock_dispense.io_module = io_mod
rootstock_dispense.label = 'rootstock'
### XXX: right arm? why not scion arm?? the same applies for the left arm
scion_params = rospy.get_param(
'/scion') ### XXX: this must be on userdata??
scion_arm_move_group = MoveGroupInterface(
"right_arm",
fixed_frame="right_arm_base_link",
gripper_frame="right_arm_link_6")
scion_arm_move_group.setPlannerId('RRTConnectkConfigDefault')
scion_arm_move_group.setPathConstraints(path_constraints)
scion_goto_init = GoTo(sim=True)
scion_goto_init.move_group = scion_arm_move_group
scion_goto_init.params = scion_params['init']
scion_goto_init.label = 'scion'
scion_goto_pick_feeder = GoToGrid(sim=True)
scion_goto_pick_feeder.move_group = scion_arm_move_group
scion_goto_pick_feeder.params = scion_params[
'feeder'] ### XXX: name: feeder???
scion_goto_pick_feeder.label = 'scion'
scion_goto_cut = GoTo(sim=True)
scion_goto_cut.move_group = scion_arm_move_group
scion_goto_cut.params = scion_params['cut']
scion_goto_cut.label = 'scion'
scion_goto_place_clip = GoTo(sim=True)
scion_goto_place_clip.move_group = scion_arm_move_group
scion_goto_place_clip.params = scion_params['clip']
scion_goto_place_clip.label = 'scion'
scion_pick = Pick(sim=True)
scion_pick.io_module = io_mod
scion_pick.label = 'scion'
scion_cut = Cut(sim=True)
scion_cut.io_module = io_mod
scion_cut.label = 'scion'
scion_place = Place(sim=True)
scion_place.io_module = io_mod
scion_place.label = 'scion'
# Create a SMACH state machine
sm_rootstock = smach.StateMachine(
outcomes=['placed', 'failed', 'completed', 'rejected'])
sm_rootstock.userdata.params = rootstock_params
# Open the container
with sm_rootstock:
# Add states to the container
smach.StateMachine.add('GOTO_PICK',
rootstock_goto_pick_feeder,
transitions={
'reached': 'PICK',
'failed': 'failed',
'grid_completed': 'completed'
})
smach.StateMachine.add('PICK',
rootstock_pick,
transitions={
'picked': 'GOTO_CUT',
'no_plant': 'GOTO_PICK',
'failed': 'failed'
})
smach.StateMachine.add('GOTO_CUT',
rootstock_goto_cut,
transitions={
'reached': 'CUT',
'failed': 'failed'
})
smach.StateMachine.add('CUT',
rootstock_cut,
transitions={
'cutted': 'GOTO_PLACE',
'failed': 'failed'
})
smach.StateMachine.add('GOTO_PLACE',
rootstock_goto_place_clip,
transitions={
'reached': 'PLACE',
'failed': 'failed'
})
smach.StateMachine.add('PLACE',
rootstock_place,
transitions={
'placed': 'placed',
'failed': 'failed'
})
# Create a SMACH state machine
sm_scion = smach.StateMachine(
outcomes=['placed', 'failed', 'completed', 'rejected'])
sm_scion.userdata.params = scion_params
# Open the container
with sm_scion:
# Add states to the container
smach.StateMachine.add('GOTO_PICK',
scion_goto_pick_feeder,
transitions={
'reached': 'PICK',
'failed': 'failed',
'grid_completed': 'completed'
})
smach.StateMachine.add('PICK',
scion_pick,
transitions={
'picked': 'GOTO_CUT',
'no_plant': 'GOTO_PICK',
'failed': 'failed'
})
smach.StateMachine.add('GOTO_CUT',
scion_goto_cut,
transitions={
'reached': 'CUT',
'failed': 'failed'
})
smach.StateMachine.add('CUT',
scion_cut,
transitions={
'cutted': 'GOTO_PLACE',
'failed': 'failed'
})
smach.StateMachine.add('GOTO_PLACE',
scion_goto_place_clip,
transitions={
'reached': 'PLACE',
'failed': 'failed'
})
smach.StateMachine.add('PLACE',
scion_place,
transitions={
'placed': 'placed',
'failed': 'failed'
})
sm_concurrent_place = smach.Concurrence(
outcomes=['failed', 'completed', 'placed'],
default_outcome='failed',
outcome_map={
'completed': {
'PLACE_ROOTSTOCK': 'completed',
'PLACE_SCION': 'completed'
},
'placed': {
'PLACE_ROOTSTOCK': 'placed',
'PLACE_SCION': 'placed'
},
'failed': {
'PLACE_ROOTSTOCK': 'failed',
'PLACE_SCION': 'failed'
}
}, # we put them here to see the transition in the introspection
child_termination_cb=child_termination_cb,
outcome_cb=outcome_cb)
with sm_concurrent_place:
smach.Concurrence.add('PLACE_ROOTSTOCK', sm_rootstock)
smach.Concurrence.add('PLACE_SCION', sm_scion)
sm_full_operation = smach.StateMachine(outcomes=['failed', 'completed'])
sm_full_operation.userdata.params = rootstock_params
with sm_full_operation:
smach.StateMachine.add('INIT_ROOTSTOCK',
rootstock_goto_init,
transitions={
'reached': 'INIT_SCION',
'failed': 'failed'
})
smach.StateMachine.add('INIT_SCION',
scion_goto_init,
transitions={
'reached': 'CONCURRENT_PLACE',
'failed': 'failed'
})
smach.StateMachine.add('CONCURRENT_PLACE',
sm_concurrent_place,
transitions={
'placed': 'CLIP',
'completed': 'completed',
'failed': 'failed'
})
smach.StateMachine.add('CLIP',
rootstock_clip,
transitions={
'clipped': 'GOTO_INSPECTION',
'failed': 'failed'
})
smach.StateMachine.add('GOTO_INSPECTION',
rootstock_goto_inspection,
transitions={
'reached': 'INSPECTION',
'failed': 'failed'
})
smach.StateMachine.add('INSPECTION',
rootstock_inspection,
transitions={
'good': 'GOTO_DISPENSE',
'bad': 'GOTO_REJECT',
'failed': 'failed'
})
smach.StateMachine.add('GOTO_REJECT',
rootstock_goto_reject,
transitions={
'reached': 'REJECT',
'failed': 'failed'
})
smach.StateMachine.add('REJECT',
rootstock_reject,
transitions={
'rejected': 'CONCURRENT_PLACE',
'failed': 'failed'
})
smach.StateMachine.add('GOTO_DISPENSE',
rootstock_goto_dispense,
transitions={
'reached': 'DISPENSE',
'failed': 'failed'
})
smach.StateMachine.add('DISPENSE',
rootstock_dispense,
transitions={
'dispensed': 'CONCURRENT_PLACE',
'failed': 'failed'
})
sm_to_execute = sm_scion
sis = smach_ros.IntrospectionServer('server_name', sm_to_execute,
'/SM_ROOT')
sis.start()
## Execute SMACH plan
outcome = sm_to_execute.execute()
rospy.spin()
sis.stop()
class ARC_ACTION_LIB_NODE():
def __init__(self):
rospy.init_node("ARC_ACTION_LIB_NODE")
self.bug_ignore = True
# init movo groups
self._init_movo_groups()
# update sensors data
self._init_start_update_sensors()
# init servers
self._init_all_servers()
# init settings
self._init_settings()
rospy.spin()
def _init_settings(self):
self._upper_body_joints = [
"right_shoulder_pan_joint", "right_shoulder_lift_joint",
"right_arm_half_joint", "right_elbow_joint",
"right_wrist_spherical_1_joint", "right_wrist_spherical_2_joint",
"right_wrist_3_joint", "left_shoulder_pan_joint",
"left_shoulder_lift_joint", "left_arm_half_joint",
"left_elbow_joint", "left_wrist_spherical_1_joint",
"left_wrist_spherical_2_joint", "left_wrist_3_joint",
"linear_joint", "pan_joint", "tilt_joint"
]
self._right_arm_joints = [
"right_shoulder_pan_joint", "right_shoulder_lift_joint",
"right_arm_half_joint", "right_elbow_joint",
"right_wrist_spherical_1_joint", "right_wrist_spherical_2_joint",
"right_wrist_3_joint"
]
self._left_arm_joints = [
"left_shoulder_pan_joint", "left_shoulder_lift_joint",
"left_arm_half_joint", "left_elbow_joint",
"left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint",
"left_wrist_3_joint"
]
def E0_get_jointstates_pos(self):
return self.jointstates_pos
def E0_get_left_jointstates_pos(self):
return self.left_arm_js_pos
def E0_get_right_jointstates_pos(self):
return self.right_arm_js_pos
def E0_get_head_jointstates_pos(self):
return self.head_js_pos
def E0_get_linear_jointstate_pos(self):
return self.linear_js_pos
def E0_get_left_cartesian_pos(self):
return self.movegroup_larm_group.get_current_pose()
def E0_get_right_cartesian_pos(self):
return self.movegroup_rarm_group.get_current_pose()
def E0_get_tcp_pose_left(self):
lpose = self.movegroup_larm_group.get_current_pose().pose
return lpose
def E0_get_tcp_pose_right(self):
rpose = self.movegroup_rarm_group.get_current_pose().pose
return rpose
def E0_get_l_cart_force(self):
return self.cartesianforce_left
def E0_get_r_cart_force(self):
return self.cartesianforce_right
def tool_is_res_published(self, act):
return type(act.get_result()) != type(None)
def tool_is_dualres_published(self, act1, act2):
p1 = self.tool_is_res_published(act1)
p2 = self.tool_is_res_published(act2)
return p1 and p2
def tool_is_goal_done(self, act):
goal_state = act.get_state()
done_status_list = [
GoalStatus.SUCCEEDED, GoalStatus.ABORTED, GoalStatus.REJECTED,
GoalStatus.RECALLED
]
if (goal_state in done_status_list):
sub_goal_done = True
# print("sub_goal_done, status =", sub_goal_done)
else:
sub_goal_done = False
return sub_goal_done
def tool_is_dualgoal_done(self, act1, act2):
sub_goal_done_l = self.tool_is_goal_done(act=act1)
sub_goal_done_r = self.tool_is_goal_done(act=act2)
sub_goal_done = sub_goal_done_l and sub_goal_done_r
return sub_goal_done
def tool_is_dualaction_success(self, act1, act2):
left_move_success = act1.get_result()
right_move_success = act2.get_result()
try:
right_done_success = right_move_success.error_code.val == MoveItErrorCodes.SUCCESS
left_done_success = left_move_success.error_code.val == MoveItErrorCodes.SUCCESS
done_success = right_done_success and left_done_success
except Exception as err:
done_success = False
return done_success
def tool_is_forcesuccess(self, lmaxforce, rmaxforce):
# lmaxforce = goal.l_max_force
# rmaxforce = goal.r_max_force
lforce = self.E0_get_l_cart_force()
rforce = self.E0_get_r_cart_force()
force_range_detect_left = [(abs(lmaxforce[i]) - abs(lforce[i]) < 0)
for i in range(6)]
force_range_detect_right = [(abs(rmaxforce[i]) - abs(rforce[i]) < 0)
for i in range(6)]
force_range_detect = [
force_range_detect_left[i] or force_range_detect_right[i]
for i in range(6)
]
done_shut = rospy.is_shutdown()
force_success = sum(force_range_detect) > 0
return force_success
def _init_movo_groups(self):
self.move_group = MoveGroupInterface("upper_body", "base_link")
self.lmove_group = MoveGroupInterface("left_arm", "base_link")
self.rmove_group = MoveGroupInterface("right_arm", "base_link")
self.move_group.setPlannerId("RRTConnectkConfigDefault")
self.lmove_group.setPlannerId("RRTConnectkConfigDefault")
self.rmove_group.setPlannerId("RRTConnectkConfigDefault")
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
group_names = self.robot.get_group_names()
print("Avaliable Groups_name", group_names)
self.movegroup_upper_group = moveit_commander.MoveGroupCommander(
"upper_body")
self.movegroup_rarm_group = moveit_commander.MoveGroupCommander(
"right_arm")
self.movegroup_larm_group = moveit_commander.MoveGroupCommander(
"left_arm")
# # time.sleep(15)
# # time.sleep(15)
self.display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20) # For trajectory publish in rviz display
print("All groups are inited!")
def _init_all_servers(self):
self.Server_L0_upper_jp_move_safe = L0_upper_jp_move_safe_srv(self)
self.Server_L0_dual_jp_move_safe_relate = L0_dual_jp_move_safe_relate_srv(
self)
self.Server_L0_dual_task_move_safe_relate = L0_dual_task_move_safe_relate_srv(
self)
self.Server_L0_dual_set_gripper = L0_dual_set_gripper_srv(self)
def _init_start_update_sensors(self):
self._subscribe_force("right")
self._subscribe_force("left")
self._subscribe_jointstates()
self.cartesianforce_right = None
self.cartesianforce_left = None
self.jointstates_pos = None
self.jointstates_vel = None
self.jointstates_effort = None
self.right_arm_js_pos = None
self.left_arm_js_pos = None
self.right_arm_js_vel = None
self.left_arm_js_vel = None
self.head_js_pos = None
self.head_js_vel = None
self.linear_js_pos = None
self.linear_js_vel = None
# When you add new states, please regist in _states
# Wait till all states got updated
while (sum(type(i) == type(None) for i in self._states)):
print(self._states)
time.sleep(1)
# print(self.jointstates_effort)
print(" Wait ... till all states got first updated")
print(" All States Got Updated at the first time!")
@property
def _states(self):
return [
self.cartesianforce_left, self.cartesianforce_right,
self.jointstates_pos, self.jointstates_vel,
self.jointstates_effort, self.right_arm_js_pos,
self.left_arm_js_pos, self.right_arm_js_vel, self.left_arm_js_vel,
self.head_js_pos, self.head_js_vel, self.linear_js_pos,
self.linear_js_vel
]
def _subscribe_force_callback_right(self, data):
fx = data.x
fy = data.y
fz = data.z
fr = data.theta_x
fp = data.theta_y
fa = data.theta_z
# print data
self.cartesianforce_right = [fx, fy, fz, fr, fp, fa]
def _subscribe_force_callback_left(self, data):
fx = data.x
fy = data.y
fz = data.z
fr = data.theta_x
fp = data.theta_y
fa = data.theta_z
# print data
self.cartesianforce_left = [fx, fy, fz, fr, fp, fa]
# print(self.cartesianforce)
def _subscribe_force(self, arm):
assert arm in ["left", "right"]
arm_name = arm + '_arm'
# print "======== I'm subscribe_force =========="
if (arm == "left"):
rospy.Subscriber("/movo/{}/cartesianforce".format(arm_name),
JacoCartesianVelocityCmd,
self._subscribe_force_callback_left)
elif (arm == "right"):
rospy.Subscriber("/movo/{}/cartesianforce".format(arm_name),
JacoCartesianVelocityCmd,
self._subscribe_force_callback_right)
def _subscribe_jointstates_callback(self, data):
# [linear_joint, pan_joint, tilt_joint,
# mid_body_joint,
# right_shoulder_pan_joint, right_shoulder_lift_joint,right_arm_half_joint,
# right_elbow_joint, right_wrist_spherical_1_joint, right_wrist_spherical_2_joint,
# right_wrist_3_joint,
# left_shoulder_pan_joint, left_shoulder_lift_joint, left_arm_half_joint,
# left_elbow_joint, left_wrist_spherical_1_joint, left_wrist_spherical_2_joint,
# left_wrist_3_joint,
# right_gripper_finger1_joint, right_gripper_finger2_joint, right_gripper_finger3_joint,
# left_gripper_finger1_joint, left_gripper_finger2_joint, left_gripper_finger3_joint]
self.jointstates_pos = data.position
self.jointstates_vel = data.velocity
self.jointstates_effort = data.effort
self.linear_js_pos = data.position[0:1][0]
self.linear_js_vel = data.velocity[0:1][0]
self.head_js_pos = data.position[1:3] #
self.head_js_vel = data.velocity[1:3] #
self.right_arm_js_pos = data.position[4:11]
self.right_arm_js_vel = data.velocity[4:11]
self.left_arm_js_pos = data.position[11:18]
self.left_arm_js_vel = data.velocity[11:18]
def _subscribe_jointstates(self):
rospy.Subscriber("/joint_states", JointState,
self._subscribe_jointstates_callback)
class screw_1(object):
def __init__(self):
# self.scene = PlanningSceneInterface("base_link")
self.dof = "7dof"
self.robot = moveit_commander.RobotCommander()
self._listener = tf.TransformListener()
self.move_group = MoveGroupInterface("upper_body", "base_link")
self.lmove_group = MoveGroupInterface("left_arm", "base_link")
self.rmove_group = MoveGroupInterface("right_arm", "base_link")
self.move_group.setPlannerId("RRTConnectkConfigDefault")
self.lmove_group.setPlannerId("RRTConnectkConfigDefault")
self.rmove_group.setPlannerId("RRTConnectkConfigDefault")
if "7dof" == self.dof:
self._upper_body_joints = [
"right_shoulder_pan_joint", "right_shoulder_lift_joint",
"right_arm_half_joint", "right_elbow_joint",
"right_wrist_spherical_1_joint",
"right_wrist_spherical_2_joint", "right_wrist_3_joint",
"left_shoulder_pan_joint", "left_shoulder_lift_joint",
"left_arm_half_joint", "left_elbow_joint",
"left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint",
"left_wrist_3_joint", "linear_joint", "pan_joint", "tilt_joint"
]
self._right_arm_joints = [
"right_shoulder_pan_joint", "right_shoulder_lift_joint",
"right_arm_half_joint", "right_elbow_joint",
"right_wrist_spherical_1_joint",
"right_wrist_spherical_2_joint", "right_wrist_3_joint"
]
self._left_arm_joints = [
"left_shoulder_pan_joint", "left_shoulder_lift_joint",
"left_arm_half_joint", "left_elbow_joint",
"left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint",
"left_wrist_3_joint"
]
self.tucked = [
-1.6, -1.5, 0.4, -2.7, 0.0, 0.5, -1.7, 1.6, 1.5, -0.4, 2.7,
0.0, -0.5, 1.7, 0.04, 0, 0
]
# self.constrained_stow =[-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, 1.0, 2.6, -2.0, 0.0, -2.0, 0.0, 0.0, -1.0, 0.42, 0, 0]
self.constrained_stow = [
-2.037, 1.046, -2.877, -1.423, -1.143, 1.679, 1.690, 2.037,
-1.046, 2.877, 1.423, 1.143, -1.679, -1.690, 0.4, 0.288, 0
]
else:
rospy.logerr("DoF needs to be set at 7, aborting demo")
return
self._lgripper = GripperActionClient('left')
self._rgripper = GripperActionClient('right')
# self.scene.clear()
def goto_tuck(self):
# remove previous objects
raw_input("========== Press Enter to goto_tuck ========")
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(
self._upper_body_joints,
self.tucked,
0.05,
max_velocity_scaling_factor=0.4)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def goto_plan_grasp(self):
raw_input("========== Press Enter to goto_plan_grasp ========")
while not rospy.is_shutdown():
try:
result = self.move_group.moveToJointPosition(
self._upper_body_joints,
self.constrained_stow,
0.05,
max_velocity_scaling_factor=0.3)
except:
continue
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def insert_bolt(self):
# right arm
# base_link y+
pose = self.curr_pose('right_ee_link')
pose.pose.position.y += 0.05
raw_input("=========== Press Enter to insert bolt ========")
# rospy.sleep(2.0)
while not rospy.is_shutdown():
try:
result = self.rmove_group.moveToPose(
pose,
"right_ee_link",
tolerance=0.02,
max_velocity_scaling_factor=0.1)
except:
continue
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def insert_nut(self):
pass
def start(self):
pass
def retreat(self):
pass
def curr_pose(self, eef):
pose = PoseStamped()
pose.header.frame_id = eef
curr_pose = self._listener.transformPose('/base_link', pose)
return curr_pose
