def plan_and_execute(self, userdata):
### DEBUG PERCEPTION ONLY
#sss.wait_for_input()
#userdata.new_box = True
#return True
### END DEBUG
# print "The pose reference frame is",self.mgc.get_pose_reference_frame()
# self.mgc.set_pose_reference_frame("camera_rgb_optical_frame")
# print "The pose reference frame is",self.mgc.get_pose_reference_frame()
start_plan = rospy.Time.now()
### Set next (virtual) start state
start_state = RobotState()
(pre_grasp_config, error_code) = sss.compose_trajectory("arm","pre_grasp")
if error_code != 0:
rospy.logerr("unable to parse pre_grasp configuration")
return False
start_state.joint_state.name = pre_grasp_config.joint_names
start_state.joint_state.position = pre_grasp_config.points[0].positions
start_state.is_diff = True
# self.mgc.set_start_state(start_state)
self.mgc.set_start_state_to_current_state()
### Plan Approach
approach_pose_offset = PoseStamped()
approach_pose_offset.header.frame_id = "current_box"
approach_pose_offset.header.stamp = rospy.Time(0)
# approach_pose_offset.pose.position.x -= 0.1
approach_pose_offset.pose.position.z += 0.05
#quat = tf.transformations.quaternion_from_euler(0, 1.57, 1.57)
#approach_pose_offset.pose.orientation.x = quat[0]
#approach_pose_offset.pose.orientation.y = quat[1]
#approach_pose_offset.pose.orientation.z = quat[2]
#approach_pose_offset.pose.orientation.w = quat[3]
try:
approach_pose = self.listener.transformPose("table", approach_pose_offset)
approach_pose.pose.position.x += -0.01
# approach_pose = self.listener.transformPose("camera_rgb_optical_frame", approach_pose_offset)
except Exception, e:
rospy.logerr("could not transform pose. Exception: %s", str(e))
return False
def get_robot_state(self):
# pose = pose_from_trans(self.get_world_pose(BASE_FRAME)) # TODO: could get this transform directly
# transform = Transform(Vector3(*point_from_pose(pose)), Quaternion(*quat_from_pose(pose)))
# transform = self.get_transform()
# if transform is None:
# return None
state = RobotState()
state.joint_state = self.joint_state
# state.multi_dof_joint_state.header.frame_id = '/base_footprint'
# state.multi_dof_joint_state.header.stamp = rospy.Time(0)
# state.multi_dof_joint_state.joints = ['world_joint']
# state.multi_dof_joint_state.transforms = [transform]
# 'world_joint'
# http://cram-system.org/tutorials/intermediate/moveit
state.attached_collision_objects = []
state.is_diff = False
# rostopic info /joint_states
return state
def _get_ik_pykdl(self, eef_poses, seeds=(), params=None):
solutions = []
for pose_num, eef_pose in enumerate(eef_poses):
if eef_pose.header.frame_id.strip('/') != self._world.strip('/'):
raise NotImplementedError("_get_ik_pykdl: Baxter PyKDL implementation does not accept frame_ids other than {}".format(self._world))
pose = pose_to_list(eef_pose)
resp = self._kinematics_pykdl.inverse_kinematics(pose[0], pose[1],
[seeds[pose_num].joint_state.position[seeds[pose_num].joint_state.name.index(joint)]
for joint in self.joint_names()] if len(seeds)>0 else None)
if resp is None:
rs = None
else:
rs = RobotState()
rs.is_diff = False
rs.joint_state.name = self.joint_names()
rs.joint_state.position = resp
solutions.append(rs)
return solutions
def plan_and_execute(self, userdata):
# add table
ps = PoseStamped()
ps.header.frame_id = "table_top"
ps.pose.position.x = -0.05
ps.pose.position.z = 0.05
ps.pose.orientation.w = 1
filename = rospkg.RosPack().get_path(
"hmi_manipulation") + "/files/hmi_table.stl"
self.psi.add_mesh("table", ps, filename)
### Set next (virtual) start state
start_state = RobotState()
(pre_grasp_config,
error_code) = sss.compose_trajectory("arm_" + userdata.active_arm,
"pre_grasp")
if error_code != 0:
rospy.logerr("unable to parse pre_grasp configuration")
return False
start_state.joint_state.name = pre_grasp_config.joint_names
start_state.joint_state.position = pre_grasp_config.points[0].positions
start_state.is_diff = True
if userdata.active_arm == "left":
self.mgc_left.set_start_state(start_state)
elif userdata.active_arm == "right":
self.mgc_right.set_start_state(start_state)
else:
rospy.logerr("invalid arm_active")
return False
### Plan Approach
approach_pose_offset = PoseStamped()
approach_pose_offset.header.frame_id = "current_rose"
approach_pose_offset.header.stamp = rospy.Time(0)
approach_pose_offset.pose.position.x = -0.12
approach_pose_offset.pose.orientation.w = 1
try:
approach_pose = self.listener.transformPose(
"odom_combined", approach_pose_offset)
except Exception, e:
rospy.logerr("could not transform pose. Exception: %s", str(e))
return False
def _get_ik_pykdl(self, eef_poses, seeds=(), params=None):
solutions = []
for pose_num, eef_pose in enumerate(eef_poses):
if eef_pose.header.frame_id.strip('/') != self._world.strip('/'):
raise NotImplementedError(
"_get_ik_pykdl: Baxter PyKDL implementation does not accept frame_ids other than {}"
.format(self._world))
pose = pose_to_list(eef_pose)
resp = self._kinematics_pykdl.inverse_kinematics(
pose[0], pose[1], [
seeds[pose_num].joint_state.position[
seeds[pose_num].joint_state.name.index(joint)]
for joint in self.joint_names()
] if len(seeds) > 0 else None)
if resp is None:
rs = None
else:
rs = RobotState()
rs.is_diff = False
rs.joint_state.name = self.joint_names()
rs.joint_state.position = resp
solutions.append(rs)
return solutions
### Initialize simple_script_server
sss = simple_script_server()
#sss.move("arm_right", "side")
#sss.move("gripper_right", "close")
### Set next (virtual) start state
start_state = RobotState()
(pre_grasp_config,
error_code) = sss.compose_trajectory("arm_right", "pre_grasp")
if error_code != 0:
rospy.logerr("unable to parse pre_grasp configuration")
sys.exit()
start_state.joint_state.name = pre_grasp_config.joint_names
start_state.joint_state.position = pre_grasp_config.points[0].positions
start_state.is_diff = True
### Plan Approach
# transform rose into arm_7_link
try:
(trans, rot) = listener.lookupTransform("odom_combined",
"current_rose", rospy.Time(0))
goal_pose = Pose()
goal_pose.position.x = trans[0]
goal_pose.position.y = trans[1]
goal_pose.position.z = trans[2]
goal_pose.orientation.x = rot[0]
goal_pose.orientation.y = rot[1]
goal_pose.orientation.z = rot[2]
goal_pose.orientation.w = rot[3]
