def _final_odom_to_robot_pose(self, traj): idxs = [ i for i, n in enumerate(traj.joint_names) if n in self._base_joint_names ] point = traj.points[-1] return geometry.tuples_to_pose( ((point.positions[idx[0]], point.positions[idx[1]], 0), (0, 0, point.positions[idx[2]])))
def _lookup_odom_to_ref(self, ref_frame_id): """Resolve current reference frame transformation from ``odom``. Returns: geometry_msgs.msg.Pose: A transform from robot ``odom`` to ``ref_frame_id``. """ odom_to_ref_ros = self._tf2_buffer.lookup_transform( settings.get_frame('odom'), ref_frame_id, rospy.Time(0), rospy.Duration(_TF_TIMEOUT)).transform odom_to_ref_tuples = geometry.transform_to_tuples(odom_to_ref_ros) return geometry.tuples_to_pose(odom_to_ref_tuples)
def move(self, pose, timeout=0, ref_frame_id=None, angular_thresh=10.0, spatial_thresh=0.1): """ Move to a pose in the map frame within some angle and linear epsilon :param pose: :param timeout: :param ref_frame_id: :param angular_thresh: degrees :param spatial_thresh: meters """ mobile_base._validate_timeout(timeout) map_frame = settings.get_frame('map') goal_pose = PoseStamped() goal_pose.pose = geometry.tuples_to_pose(pose) # Get goal position relative to the map if ref_frame_id: goal_pose.header.frame_id = ref_frame_id transform = self.omni_base._tf2_buffer.lookup_transform(map_frame, ref_frame_id, rospy.Time(0), rospy.Duration(1)) goal_pose = tf2_geometry_msgs.do_transform_pose(goal_pose, transform) goal_pose.header.frame_id = map_frame goal_pose.header.stamp = rospy.Time(0) goal = MoveBaseGoal() goal.target_pose = goal_pose ac = self.omni_base._action_client ac.send_goal(goal) end_time = rospy.Time.now() + rospy.Duration(timeout) try: # Wait for a few milliseconds and check again while not ac.wait_for_result(rospy.Duration(0.1)): angular_dist = self.__quaternion_dist(self.global_pose.pose.orientation, goal_pose.pose.orientation) linear_dist = self.__euclid_dist(self.global_pose.pose.position, goal_pose.pose.position) if angular_dist <= angular_thresh and linear_dist <= spatial_thresh: ac.cancel_goal() elif timeout > 0 and rospy.Time.now() > end_time: ac.cancel_goal() raise RuntimeError("Fast move timed out!") final_state = ac.get_state() # FIXME: Not sure why the final state is sometimes GoalStatus.ACTIVE if final_state in [GoalStatus.PREEMPTED, GoalStatus.PREEMPTING, GoalStatus.PENDING, GoalStatus.ACTIVE]: rospy.loginfo("FastMove preempted motion. Final state {}:{}".format(final_state, ac.get_goal_status_text())) return elif ac.get_state() != actionlib.GoalStatus.SUCCEEDED: raise RuntimeError("Failed {}: {}".format(ac.get_state(), ac.get_goal_status_text())) except KeyboardInterrupt: ac.cancel_goal()
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 call_tsr_plan_service(whole_body, constraint_tsrs, goal_tsrs): odom_to_robot_pose = geometry.tuples_to_pose(get_relative_tuples(_ORIGIN_TF, _ROBOT_TF)) req = PlanWithTsrConstraintsRequest() req.base_movement_type.val = BaseMovementType.PLANAR req.origin_to_basejoint = odom_to_robot_pose req.initial_joint_state = whole_body._get_joint_state() req.use_joints = _USE_JOINTS req.probability_goal_generate = _PLANNING_GOAL_GENERATION req.timeout = rospy.Duration(whole_body._planning_timeout) req.max_iteration = _PLANNING_MAX_ITERATION req.uniform_bound_sampling = False req.deviation_for_bound_sampling = _PLANNING_GOAL_DEVIATION req.weighted_joints = ['_linear_base', '_rotational_base'] req.weight = [whole_body._linear_weight, whole_body._angular_weight] if whole_body._collision_world is not None: req.environment_before_planning = whole_body._collision_world.snapshot(_ORIGIN_TF) req.constraint_tsrs = constraint_tsrs req.goal_tsrs = goal_tsrs plan_service = rospy.ServiceProxy('plan_with_constraints', PlanWithTsrConstraints)
def main(whole_body, gripper, wrist_wrench): # Grab the handle of door # wrist_wrench =wholjjjjjjjjjk whole_body.move_to_neutral() switch = ImpedanceControlSwitch() wrist_wrench.reset() gripper.command(1.0) grab_pose = geometry.multiply_tuples( geometry.pose(x=HANDLE_POS[0] - HANDLE_TO_HAND_POS, y=HANDLE_POS[1], z=HANDLE_POS[2], ej=math.pi / 2), geometry.pose(ek=math.pi / 2)) whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF) switch.activate("grasping") # gripper.command(0.01) gripper.grasp(-0.05) rospy.sleep(1.0) switch.inactivate() rospy.sleep(10.0) listener = tf.TransformListener() listener.waitForTransform("/map", "/hand_palm_link", rospy.Time(), rospy.Duration(4.0)) now = rospy.Time.now() listener.waitForTransform("/map", "/hand_palm_link", now, rospy.Duration(4.0)) # (trans, rot) = listener.lookupTransform("/map", "/hand_palm_link", now) # cur_tuples = geometry.transform_to_tuples(target_trans) # print trans # print rot #tf frame # br = tf2_ros.TransformBroadcaster() # t = geometry_msgs.msg.TransformStamped() # t.header.stamp = rospy.Time.now() # t.header.frame_id = "/map" # t.child_frame_id = "/target" # t.transform.translation.x = trans[0] # t.transform.translation.y = trans[1] # t.transform.translation.z = trans[2] # q = tf.transformations.quaternion_from_euler(0, 0, 0.0) # t.transform.rotation.x = rot[0] # t.transform.rotation.y = rot[1] # t.transform.rotation.z = rot[2] # t.transform.rotation.w = rot[3] # br.sendTransform(t) # Rotate the handle (Angle: math.pi/6) odom_to_hand = get_relative_tuples(_ORIGIN_TF, _HAND_TF) tsr_to_odom = geometry.pose( x=-(HANDLE_POS[0] + HANDLE_TO_HAND_POS), y=-(HANDLE_POS[1] + HANDLE_TO_HANDLE_HINGE_POS), z=-HANDLE_POS[2]) tsr_to_hand = geometry.multiply_tuples(tsr_to_odom, odom_to_hand) const_tsr = TaskSpaceRegion() const_tsr.end_frame_id = _HAND_TF const_tsr.origin_to_tsr = geometry.tuples_to_pose( geometry.pose(x=HANDLE_POS[0] + HANDLE_TO_HAND_POS, y=HANDLE_POS[1] + HANDLE_TO_HANDLE_HINGE_POS, z=HANDLE_POS[2])) const_tsr.tsr_to_end = geometry.tuples_to_pose(tsr_to_hand) const_tsr.min_bounds = [0, 0.0, 0.0, -(math.pi / 5), 0, 0] const_tsr.max_bounds = [0, 0.0, 0.0, 0, 0, 0] goal_tsr = TaskSpaceRegion() goal_tsr.end_frame_id = _HAND_TF goal_tsr.origin_to_tsr = geometry.tuples_to_pose( geometry.pose(x=HANDLE_POS[0] + HANDLE_TO_HAND_POS, y=HANDLE_POS[1] + HANDLE_TO_HANDLE_HINGE_POS, z=HANDLE_POS[2])) goal_tsr.tsr_to_end = geometry.tuples_to_pose(tsr_to_hand) goal_tsr.min_bounds = [0, 0.0, 0.0, -math.pi / 5, 0, 0] goal_tsr.max_bounds = [0, 0.0, 0.0, -math.pi / 5, 0, 0] response = call_tsr_plan_service(whole_body, [const_tsr], [goal_tsr]) if response.error_code.val != ArmManipulationErrorCodes.SUCCESS: rospy.logerr("Planning failed: (Error Code {0})".format( response.error_code.val)) exit(-1) response.base_solution.header.frame_id = _ORIGIN_TF constrain_traj = whole_body._constrain_trajectories( response.solution, response.base_solution) whole_body._execute_trajectory(constrain_traj) rospy.sleep(10.0) # listener = tf.TransformListener() now = rospy.Time.now() listener.waitForTransform("/map", "/hand_palm_link", now, rospy.Duration(4.0)) rospy.sleep(1.0)
def _plan_cartesian_path(self, origin_to_pose1, origin_to_pose2, odom_to_robot_pose, initial_joint_state, collision_env): use_joints = (b'wrist_flex_joint', b'wrist_roll_joint', b'arm_roll_joint', b'arm_flex_joint', b'arm_lift_joint') req = PlanWithTsrConstraintsRequest() req.origin_to_basejoint = odom_to_robot_pose req.initial_joint_state = initial_joint_state req.base_movement_type.val = BaseMovementType.PLANAR req.use_joints = use_joints req.weighted_joints = [b'_linear_base', b'_rotational_base'] req.weight = [self._linear_weight, self._angular_weight] req.probability_goal_generate = _PLANNING_GOAL_GENERATION req.attached_objects = [] if collision_env is not None: req.environment_before_planning = collision_env req.timeout = rospy.Duration(self._planning_timeout) req.max_iteration = _PLANNING_MAX_ITERATION req.uniform_bound_sampling = False req.deviation_for_bound_sampling = _PLANNING_GOAL_DEVIATION req.extra_constraints = [] req.extra_goal_constraints = [] move_axis, distance = _movement_axis_and_distance( origin_to_pose1, origin_to_pose2) origin_to_axis = _pose_from_x_axis(move_axis) pose1_to_axis = geometry.multiply_tuples(_invert_pose(origin_to_pose1), origin_to_axis) pose1_to_axis = geometry.Pose((0, 0, 0), pose1_to_axis[1]) origin_to_tsr = geometry.multiply_tuples(origin_to_pose1, pose1_to_axis) tsr_to_pose1 = _invert_pose(pose1_to_axis) # Goal constraint tsr_g = TaskSpaceRegion() tsr_g.end_frame_id = bytes(self.end_effector_frame) tsr_g.origin_to_tsr = geometry.tuples_to_pose(origin_to_pose2) tsr_g.tsr_to_end = geometry.tuples_to_pose(geometry.pose()) tsr_g.min_bounds = [0, 0, 0, 0, 0, 0] tsr_g.max_bounds = [0, 0, 0, 0, 0, 0] # Line constraint tsr_c = TaskSpaceRegion() tsr_c.end_frame_id = bytes(self.end_effector_frame) tsr_c.origin_to_tsr = geometry.tuples_to_pose(origin_to_tsr) tsr_c.tsr_to_end = geometry.tuples_to_pose(tsr_to_pose1) tsr_c.min_bounds = [0, 0, 0, -math.pi, -math.pi, -math.pi] tsr_c.max_bounds = [distance, 0, 0, math.pi, math.pi, math.pi] req.goal_tsrs = [tsr_g] req.constraint_tsrs = [tsr_c] service_name = self._setting['plan_with_constraints_service'] plan_service = rospy.ServiceProxy(service_name, PlanWithTsrConstraints) res = plan_service.call(req) print(res) if res.error_code.val != ArmManipulationErrorCodes.SUCCESS: msg = "Fail to plan" print(msg) print(req) raise exceptions.MotionPlanningError(msg, res.error_code) return res
def main(whole_body, gripper): # Grab the handle of door whole_body.move_to_neutral() gripper.command(1.0) grab_pose = geometry.multiply_tuples(geometry.pose(x=HANDLE_POS[0]-HANDLE_TO_HAND_POS, y=HANDLE_POS[1], z=HANDLE_POS[2], ej=math.pi/2), geometry.pose(ek=math.pi/2)) whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF) gripper.command(0.1) rospy.sleep(10.0) # Rotate the handle (Angle: math.pi/6) odom_to_hand = get_relative_tuples(_ORIGIN_TF, _HAND_TF) tsr_to_odom = geometry.pose(x=-(HANDLE_POS[0]-HANDLE_TO_HAND_POS), y=-(HANDLE_POS[1]-HANDLE_TO_HANDLE_HINGE_POS), z=-HANDLE_POS[2]) tsr_to_hand = geometry.multiply_tuples(tsr_to_odom, odom_to_hand) const_tsr = TaskSpaceRegion() const_tsr.end_frame_id = _HAND_TF const_tsr.origin_to_tsr = geometry.tuples_to_pose(geometry.pose(x=HANDLE_POS[0]-HANDLE_TO_HAND_POS, y=HANDLE_POS[1]-HANDLE_TO_HANDLE_HINGE_POS, z=HANDLE_POS[2])) const_tsr.tsr_to_end = geometry.tuples_to_pose(tsr_to_hand) const_tsr.min_bounds = [0, 0.0, 0.0, 0, 0, 0] const_tsr.max_bounds = [0, 0.0, 0.0, math.pi/6, 0, 0] goal_tsr = TaskSpaceRegion() goal_tsr.end_frame_id = _HAND_TF goal_tsr.origin_to_tsr = geometry.tuples_to_pose(geometry.pose(x=HANDLE_POS[0]-HANDLE_TO_HAND_POS, y=HANDLE_POS[1]-HANDLE_TO_HANDLE_HINGE_POS, z=HANDLE_POS[2])) goal_tsr.tsr_to_end = geometry.tuples_to_pose(tsr_to_hand) goal_tsr.min_bounds = [0, 0.0, 0.0, math.pi/6, 0, 0] goal_tsr.max_bounds = [0, 0.0, 0.0, math.pi/6, 0, 0] response = call_tsr_plan_service(whole_body, [const_tsr], [goal_tsr]) if response.error_code.val != ArmManipulationErrorCodes.SUCCESS: rospy.logerr("Planning failed: (Error Code {0})".format(response.error_code.val)) exit(-1) response.base_solution.header.frame_id = _ORIGIN_TF constrain_traj = whole_body._constrain_trajectories(response.solution, response.base_solution) whole_body._execute_trajectory(constrain_traj) rospy.sleep(10.0) # Open the door (Angle: math.pi/4) odom_to_hand = get_relative_tuples(_ORIGIN_TF, _HAND_TF) #T0h tsr_to_odom = geometry.pose(x=-HANDLE_POS[0], y=-(HANDLE_POS[1]-HANDLE_TO_DOOR_HINGE_POS), z=-HANDLE_POS[2]) #Twe tsr_to_hand = geometry.multiply_tuples(tsr_to_odom, odom_to_hand) #T0s' const_tsr = TaskSpaceRegion() const_tsr.end_frame_id = _HAND_TF const_tsr.origin_to_tsr = geometry.tuples_to_pose(geometry.pose(x=HANDLE_POS[0], y=HANDLE_POS[1]-HANDLE_TO_DOOR_HINGE_POS, z=HANDLE_POS[2])) const_tsr.tsr_to_end = geometry.tuples_to_pose(tsr_to_hand) const_tsr.min_bounds = [0, 0.0, 0.0, 0, 0, 0] const_tsr.max_bounds = [0, 0.0, 0.0, 0, 0, math.pi/4] goal_tsr = TaskSpaceRegion() goal_tsr.end_frame_id = _HAND_TF goal_tsr.origin_to_tsr = geometry.tuples_to_pose(geometry.pose(x=HANDLE_POS[0], y=HANDLE_POS[1]-HANDLE_TO_DOOR_HINGE_POS, z=HANDLE_POS[2])) goal_tsr.tsr_to_end = geometry.tuples_to_pose(tsr_to_hand) goal_tsr.min_bounds = [0, 0.0, 0.0, 0, 0, math.pi/4] goal_tsr.max_bounds = [0, 0.0, 0.0, 0, 0, math.pi/4] response = call_tsr_plan_service(whole_body, [const_tsr], [goal_tsr]) if response.error_code.val != ArmManipulationErrorCodes.SUCCESS: rospy.logerr("Planning failed: (Error Code {0})".format(response.error_code.val)) exit(-1) response.base_solution.header.frame_id = _ORIGIN_TF constrain_traj = whole_body._constrain_trajectories(response.solution, response.base_solution) whole_body._execute_trajectory(constrain_traj)
def main(whole_body, gripper): # Grab the handle of door whole_body.move_to_neutral() gripper.command(1.0) #open hand grab_pose = geometry.multiply_tuples(geometry.pose(x=HANDLE_POS[0]-HANDLE_TO_HAND_POS, y=HANDLE_POS[1], z=HANDLE_POS[2], ej=(math.pi/2)), geometry.pose(ek=-(math.pi/2))) whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF) #the frame that is used is specified gripper.grasp(-1.0) #close hand rospy.sleep(10.0) # Rotate the handle (Angle: math.pi/6) odom_to_hand = get_relative_tuples(_ORIGIN_TF, _HAND_TF) tsr_to_odom = geometry.pose(x=-(HANDLE_POS[0]-HANDLE_TO_HAND_POS), y=-(HANDLE_POS[1]+HANDLE_TO_HANDLE_HINGE_POS), z=-HANDLE_POS[2]) tsr_to_hand = geometry.multiply_tuples(tsr_to_odom, odom_to_hand) #creation of constraints const_tsr = TaskSpaceRegion() const_tsr.end_frame_id = _HAND_TF #I'm in the hand frame const_tsr.origin_to_tsr = geometry.tuples_to_pose(geometry.pose(x=HANDLE_POS[0]-HANDLE_TO_HAND_POS, y=HANDLE_POS[1]+HANDLE_TO_HANDLE_HINGE_POS, z=HANDLE_POS[2])) const_tsr.tsr_to_end = geometry.tuples_to_pose(tsr_to_hand) const_tsr.min_bounds = [0, 0.0, 0.0, -(math.pi/6), 0, 0] const_tsr.max_bounds = [0, 0.0, 0.0, 0, 0, 0] #creation of the goal position goal_tsr = TaskSpaceRegion() goal_tsr.end_frame_id = _HAND_TF #I'm in the hand frame goal_tsr.origin_to_tsr = geometry.tuples_to_pose(geometry.pose(x=HANDLE_POS[0]-HANDLE_TO_HAND_POS, y=HANDLE_POS[1]+HANDLE_TO_HANDLE_HINGE_POS, z=HANDLE_POS[2])) goal_tsr.tsr_to_end = geometry.tuples_to_pose(tsr_to_hand) goal_tsr.min_bounds = [0, 0.0, 0.0, -(math.pi/6), 0, 0] goal_tsr.max_bounds = [0, 0.0, 0.0, -(math.pi/6), 0, 0] #computation of the trajectory response = call_tsr_plan_service(whole_body, [const_tsr], [goal_tsr]) #make sure no errors if response.error_code.val != ArmManipulationErrorCodes.SUCCESS: rospy.logerr("Planning failed: (Error Code {0})".format(response.error_code.val)) exit(-1) response.base_solution.header.frame_id = _ORIGIN_TF constrain_traj = whole_body._constrain_trajectories(response.solution, response.base_solution) #EXECUTION of the handle rotation whole_body._execute_trajectory(constrain_traj) rospy.sleep(10.0) #at this point the handle should be open # Open the door (Angle: math.pi/4) odom_to_hand = get_relative_tuples(_ORIGIN_TF, _HAND_TF) tsr_to_odom = geometry.pose(x=-HANDLE_POS[0], y=-(HANDLE_POS[1]+HANDLE_TO_DOOR_HINGE_POS), z=-HANDLE_POS[2]) tsr_to_hand = geometry.multiply_tuples(tsr_to_odom, odom_to_hand) #constraints definition const_tsr = TaskSpaceRegion() const_tsr.end_frame_id = _HAND_TF #frame i am modifying const_tsr.origin_to_tsr = geometry.tuples_to_pose(geometry.pose(x=HANDLE_POS[0], y=HANDLE_POS[1]+HANDLE_TO_DOOR_HINGE_POS, z=HANDLE_POS[2])) const_tsr.tsr_to_end = geometry.tuples_to_pose(tsr_to_hand) const_tsr.min_bounds = [0, 0.0, 0.0, 0, 0, 0] const_tsr.max_bounds = [0, 0.0, 0.0, 0, 0, (math.pi/2)] #definition of final position goal_tsr = TaskSpaceRegion() goal_tsr.end_frame_id = _HAND_TF #frame i am modifying goal_tsr.origin_to_tsr = geometry.tuples_to_pose(geometry.pose(x=HANDLE_POS[0], y=HANDLE_POS[1]+HANDLE_TO_DOOR_HINGE_POS, z=HANDLE_POS[2])) goal_tsr.tsr_to_end = geometry.tuples_to_pose(tsr_to_hand) goal_tsr.min_bounds = [0, 0.0, 0.0, 0, 0, (math.pi/2)] goal_tsr.max_bounds = [0, 0.0, 0.0, 0, 0, (math.pi/2)] #trajectory planning response = call_tsr_plan_service(whole_body, [const_tsr], [goal_tsr]) #make sure no errors if response.error_code.val != ArmManipulationErrorCodes.SUCCESS: rospy.logerr("Planning failed: (Error Code {0})".format(response.error_code.val)) exit(-1) response.base_solution.header.frame_id = _ORIGIN_TF constrain_traj = whole_body._constrain_trajectories(response.solution, response.base_solution) #execution of results whole_body._execute_trajectory(constrain_traj)