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]