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)
