def go(self, x, y, yaw, timeout=0.0, relative=False, angular_thresh=10.0, spatial_thresh=0.1):
mobile_base._validate_timeout(timeout)
position = geometry.Vector3(x, y, 0)
quat = tf.transformations.quaternion_from_euler(0, 0, yaw)
orientation = geometry.Quaternion(*quat)
pose = (position, orientation)
if relative:
ref_frame_id = settings.get_frame('base')
else:
ref_frame_id = settings.get_frame('map')
self.move(pose, timeout, ref_frame_id, angular_thresh)
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 _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 _move_end_effector_by_line(self,
axis,
distance,
odom_to_robot_pose,
initial_joint_state,
collision_env=None):
axis_length = np.linalg.norm(np.array(axis, dtype='float64'))
if axis_length < sys.float_info.epsilon:
raise ValueError("The axis is zero vector.")
use_joints = (b'wrist_flex_joint', b'wrist_roll_joint',
b'arm_roll_joint', b'arm_flex_joint', b'arm_lift_joint')
req = PlanWithHandLineRequest()
req.base_movement_type.val = BaseMovementType.PLANAR
req.origin_to_basejoint = odom_to_robot_pose
req.initial_joint_state = initial_joint_state
req.use_joints = use_joints
req.weighted_joints = [b'_linear_base', b'_rotational_base']
req.weight = [self._linear_weight, self._angular_weight]
req.axis.x = axis[0]
req.axis.y = axis[1]
req.axis.z = axis[2]
req.local_origin_of_axis = True
req.ref_frame_id = self._end_effector_frame
req.goal_value = distance
req.probability_goal_generate = _PLANNING_GOAL_GENERATION
req.attached_objects = []
req.timeout = rospy.Duration(_PLANNING_ARM_TIMEOUT)
req.max_iteration = _PLANNING_MAX_ITERATION
req.uniform_bound_sampling = False
req.deviation_for_bound_sampling = _PLANNING_GOAL_DEVIATION
req.extra_goal_constraints = []
if collision_env is not None:
req.environment_before_planning = collision_env
service_name = self._hand_line_service
plan_service = rospy.ServiceProxy(service_name, PlanWithHandLine)
res = plan_service.call(req)
if res.error_code.val != ArmManipulationErrorCodes.SUCCESS:
msg = "Fail to plan move_hand_line"
raise exceptions.MotionPlanningError(msg, res.error_code)
res.base_solution.header.frame_id = settings.get_frame('odom')
constrained_traj = self._constrain_trajectories(
res.solution, res.base_solution, odom_to_robot_pose)
return constrained_traj
def get_end_effector_pose(self, ref_frame_id=None):
"""Get a pose of end effector based on robot frame.
Returns:
Tuple[Vector3, Quaternion]
"""
# Default reference frame is a robot frame
if ref_frame_id is None:
ref_frame_id = settings.get_frame('base')
transform = self._tf2_buffer.lookup_transform(
ref_frame_id, self._end_effector_frame, rospy.Time(0),
rospy.Duration(_TF_TIMEOUT))
result = geometry.transform_to_tuples(transform.transform)
return result
def _move_end_effector_pose(self,
odom_to_hand_pose,
odom_to_robot_pose,
initial_joint_state,
collision_env=None):
use_joints = (b'wrist_flex_joint', b'wrist_roll_joint',
b'arm_roll_joint', b'arm_flex_joint', b'arm_lift_joint')
req = PlanWithHandGoalsRequest()
req.base_movement_type.val = BaseMovementType.PLANAR
req.origin_to_basejoint = odom_to_robot_pose
req.initial_joint_state = initial_joint_state
req.use_joints = use_joints
req.weighted_joints = [b'_linear_base', b'_rotational_base']
req.weight = [self._linear_weight, self._angular_weight]
req.origin_to_hand_goals.append(odom_to_hand_pose)
req.ref_frame_id = self._end_effector_frame
req.probability_goal_generate = _PLANNING_GOAL_GENERATION
req.timeout = rospy.Duration(_PLANNING_ARM_TIMEOUT)
req.max_iteration = _PLANNING_MAX_ITERATION
req.uniform_bound_sampling = False
req.deviation_for_bound_sampling = _PLANNING_GOAL_DEVIATION
if collision_env is not None:
req.environment_before_planning = collision_env
service_name = self.hand_goal_service
plan_service = rospy.ServiceProxy(service_name, PlanWithHandGoals)
res = plan_service.call(req)
if res.error_code.val != ArmManipulationErrorCodes.SUCCESS:
msg = "Fail to plan move_endpoint"
raise exceptions.MotionPlanningError(msg, res.error_code)
res.base_solution.header.frame_id = settings.get_frame('odom')
constrained_traj = self._constrain_trajectories(
res.solution, res.base_solution, odom_to_robot_pose)
return constrained_traj
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