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()