def __init__(self, approach_frame, object_pose, grasp_pattern_msgs): self._ref_frame = object_pose.header.frame_id self._approach_frame = approach_frame self._ref_to_object = geometry.pose_to_tuples(object_pose.pose) object_to_hands = [] for grasp_pattern in grasp_pattern_msgs: object_to_hands.append( geometry.pose_to_tuples(grasp_pattern.hand_frame)) self._object_to_hands = sorted(object_to_hands, key=_distance)
def _transform_base_trajectory(self, base_traj, odom_to_frame_pose): odom_to_frame = geometry.pose_to_tuples(odom_to_frame_pose) num_points = len(base_traj.points) odom_base_traj = JointTrajectory() odom_base_traj.points = list( utils.iterate(JointTrajectoryPoint, num_points)) odom_base_traj.header = base_traj.header odom_base_traj.joint_names = self._base_joint_names # Transform each point into odom frame previous_theta = 0.0 for i in range(num_points): t = base_traj.points[i].transforms[0] frame_to_base = geometry.transform_to_tuples(t) # odom_to_base = odom_to_frame * frame_to_base (odom_to_base_trans, odom_to_base_rot) = geometry.multiply_tuples( odom_to_frame, frame_to_base) odom_base_traj.points[i].positions = [ odom_to_base_trans[0], odom_to_base_trans[1], 0 ] roll, pitch, yaw = T.euler_from_quaternion(odom_to_base_rot) dtheta = geometry.shortest_angular_distance(previous_theta, yaw) theta = previous_theta + dtheta odom_base_traj.points[i].positions[2] = theta previous_theta = theta return odom_base_traj
def manipulation_srv(self, action): """ Here the grasp precompute action (TU/e) is translated to a PlanWithHandGoals (TMC) and send as goal to the robot :param action: the GraspPrecomputeAction type """ success = True pose_quaternion = quaternion_from_euler(action.goal.roll, action.goal.pitch, action.goal.yaw) static_quaternion = quaternion_from_euler(3.14159265359, -1.57079632679, 0) final_quaternion = quaternion_multiply(pose_quaternion, static_quaternion) pose = [action.goal.x, action.goal.y, action.goal.z], final_quaternion ################################################################################################################ # This piece of code is partially copied from Toyota software, it also uses the private functions (we're very # sorry). Setting base_movement_type.val allows for adapting the allowed movements during manipulation. ref_frame_id = settings.get_frame('base') ref_to_hand_poses = [pose] odom_to_ref_pose = self.whole_body._lookup_odom_to_ref(ref_frame_id) odom_to_ref = geometry.pose_to_tuples(odom_to_ref_pose) odom_to_hand_poses = [] for ref_to_hand in ref_to_hand_poses: odom_to_hand = geometry.multiply_tuples(odom_to_ref, ref_to_hand) odom_to_hand_poses.append(geometry.tuples_to_pose(odom_to_hand)) req = self.whole_body._generate_planning_request( PlanWithHandGoalsRequest) req.origin_to_hand_goals = odom_to_hand_poses req.ref_frame_id = self.whole_body._end_effector_frame req.base_movement_type.val = BaseMovementType.ROTATION_Z service_name = self.whole_body._setting['plan_with_hand_goals_service'] plan_service = rospy.ServiceProxy(service_name, PlanWithHandGoals) res = plan_service.call(req) if res.error_code.val != ArmManipulationErrorCodes.SUCCESS: rospy.logerr('Fail to plan move_endpoint') success = False else: res.base_solution.header.frame_id = settings.get_frame('odom') constrained_traj = self.whole_body._constrain_trajectories( res.solution, res.base_solution) self.whole_body._execute_trajectory(constrained_traj) return success
def move_cartesian_path(self, waypoints, handpoints, ref_frame_id=None): if ref_frame_id is None: ref_frame_id = settings.get_frame('base') base_frame = settings.get_frame('base') origin_to_ref_ros_pose = self._lookup_odom_to_ref(ref_frame_id) ##原点のodomからのpose origin_to_ref = geometry.pose_to_tuples(origin_to_ref_ros_pose) ##原点のodomからのtuple origin_to_pose1 = self.get_end_effector_pose('odom') ##現在の手先位置 odom_to_robot_pose = self._lookup_odom_to_ref(base_frame) ##足元のodomからのpose initial_joint_state = self._get_joint_state() ##現在姿勢 if self._collision_world is not None: collision_env = self._collision_world.snapshot('odom') else: collision_env = None ##障害物設定 arm_traj = None base_traj = None p = [] for i in range(len(waypoints)): ##各動作点の計算 origin_to_pose2 = geometry.multiply_tuples(origin_to_ref, waypoints[i]) plan = self._plan_cartesian_path(origin_to_pose1, origin_to_pose2, odom_to_robot_pose, initial_joint_state, collision_env) p.append(plan) if arm_traj is None: arm_traj = plan.solution arm_traj.points = [ plan.solution.points[0], plan.solution.points[-1] ] elif len(plan.solution.points) > 0: arm_traj.points.append(plan.solution.points[-1]) if base_traj is None: base_traj = plan.base_solution base_traj.points = [ plan.base_solution.points[0], plan.base_solution.points[-1] ] elif len(plan.base_solution.points) > 0: base_traj.points.append(plan.base_solution.points[-1]) origin_to_pose1 = origin_to_pose2 odom_to_robot_pose = RosPose() final_transform = plan.base_solution.points[-1].transforms[0] odom_to_robot_pose.position.x = final_transform.translation.x odom_to_robot_pose.position.y = final_transform.translation.y odom_to_robot_pose.position.z = final_transform.translation.z odom_to_robot_pose.orientation.x = final_transform.rotation.x odom_to_robot_pose.orientation.y = final_transform.rotation.y odom_to_robot_pose.orientation.z = final_transform.rotation.z odom_to_robot_pose.orientation.w = final_transform.rotation.w initial_joint_state = plan.joint_state_after_planning collision_env = plan.environment_after_planning base_traj.header.frame_id = settings.get_frame('odom') constrained_traj = self._constrain_trajectories(arm_traj, base_traj) constrained_traj.joint_names.append("hand_motor_joint") if type(constrained_traj.points[0].positions) == tuple: constrained_traj.points[0].positions = list( constrained_traj.points[0].positions) constrained_traj.points[0].velocities = list( constrained_traj.points[0].velocities) constrained_traj.points[0].accelerations = list( constrained_traj.points[0].accelerations) constrained_traj.points[0].positions.append(handpoints[0]) constrained_traj.points[0].velocities.append(0.0) if handpoints[0] >= 1.0: constrained_traj.points[0].accelerations.append(0.1) elif handpoints[0] <= 0.1: constrained_traj.points[0].accelerations.append(-0.1) else: constrained_traj.points[0].accelerations.append(0.0) for i in range(len(handpoints)): constrained_traj.points[i + 1].positions = list( constrained_traj.points[i + 1].positions) constrained_traj.points[i + 1].velocities = list( constrained_traj.points[i + 1].velocities) constrained_traj.points[i + 1].accelerations = list( constrained_traj.points[i + 1].accelerations) constrained_traj.points[i + 1].positions.append(handpoints[i]) constrained_traj.points[i + 1].velocities.append(0.0) if handpoints[i] >= 1.0: constrained_traj.points[0].accelerations.append(0.1) elif handpoints[i] <= 0.1: constrained_traj.points[0].accelerations.append(-0.1) else: constrained_traj.points[0].accelerations.append(0.0) ##この結果の抽出え self._execute_trajectory(constrained_traj) return constrained_traj
def main(whole_body, gripper): # Initialize inv_perspective_transform_client = rospy.ServiceProxy(_INV_PERSPECTIVE_TRANSFORM_SRV_NAME, InversePerspectiveTransform) grasp_planner_client = rospy.ServiceProxy(_GRASP_PLANNER_SRV_NAME, GetGraspPattern) try: inv_perspective_transform_client.wait_for_service(timeout=_CONNECTION_TIMEOUT) grasp_planner_client.wait_for_service(timeout=_CONNECTION_TIMEOUT) except Exception as e: rospy.logerr(e) sys.exit(1) # Gaze at the target object gripper.command(1.0) whole_body.move_to_joint_positions(_VIEW_POSITIONS) rospy.sleep(1.0) # Get the position of the target object from a camera image inv_perspective_transform_req = InversePerspectiveTransformRequest() target_point = Point2DStamped() target_point.point.x = _TARGET_POINT_X target_point.point.y = _TARGET_POINT_Y inv_perspective_transform_req.points_2D.append(target_point) inv_perspective_transform_req.depth_registration = True inv_perspective_transform_req.target_frame = 'base_footprint' try: res = inv_perspective_transform_client(inv_perspective_transform_req) except rospy.ServiceException as e: rospy.logerr(e) exit(1) if len(res.points_3D) < 1: rospy.logerr('There is no detected point') exit(1) if res.points_3D[0].point.z < 0.01: rospy.logerr('The detected point is ground') exit(1) # Get the grasp patterns grasp_planner_req = GetGraspPatternRequest() grasp_planner_req.points.append(res.points_3D[0]) grasp_planner_req.search_pattern = GetGraspPatternRequest.kAbovePlane try: res = grasp_planner_client(grasp_planner_req) except rospy.ServiceException as e: rospy.logerr(e) exit(1) if len(res.grasp_patterns) < 1: rospy.logerr('There is no grasp pattern') exit(1) # Grasp the object target_hand_pose = geometry.multiply_tuples(geometry.pose_to_tuples(res.object_pose.pose), geometry.pose_to_tuples(res.grasp_patterns[0].hand_frame)) target_hand_euler = tf.transformations.euler_from_quaternion([ target_hand_pose.ori.x, target_hand_pose.ori.y, target_hand_pose.ori.z, target_hand_pose.ori.w]) target_pose = hsrb_interface.geometry.pose(x=target_hand_pose.pos.x, y=target_hand_pose.pos.y, z=target_hand_pose.pos.z, ei=target_hand_euler[0], ej=target_hand_euler[1], ek=target_hand_euler[2]) whole_body.move_end_effector_pose(target_pose) gripper.grasp(-0.1) # Move to neutral position whole_body.move_to_neutral()