def go_to_kitchen():
kitchen_pose = geometry.Pose(pos=geometry.Vector3(x=3.11, y=1.21, z=0.0),
ori=geometry.Quaternion(
x=0.0,
y=0.0,
z=-0.01023836327822357,
w=0.9999475865851083))
omni_base.go_pose(kitchen_pose)
def go_to_microwave(self):
microwave_pose = geometry.Pose(pos=geometry.Vector3(x=1.909,
y=1.779478,
z=0.0),
ori=geometry.Quaternion(x=0.0,
y=0.0,
z=0.70096336,
w=0.713197))
self.omni_base.go_pose(microwave_pose)
def go_to_yakan():
yakan_pose = geometry.Pose(pos=geometry.Vector3(x=3.108781824048564,
y=0.037752328711472494,
z=0.0),
ori=geometry.Quaternion(x=0.0,
y=0.0,
z=0.013848295774654074,
w=0.9999041077544075))
omni_base.go_pose(yakan_pose)
def go_to_stop_button(self):
stop_button_pose = geometry.Pose(
pos=geometry.Vector3(x=3.1237696626280242,
y=-0.31407748223644616,
z=0.0),
ori=geometry.Quaternion(x=0.0,
y=0.0,
z=-0.01521901673895873,
w=0.9998841840580838))
self.omni_base.go_pose(stop_button_pose)
def go_to_fridge(self):
fridge_pose = geometry.Pose(pos=geometry.Vector3(x=0.1457817782465553,
y=1.5697370723246589,
z=0.0),
ori=geometry.Quaternion(
x=0.0,
y=0.0,
z=0.7400625754780703,
w=0.6725380170494195))
self.omni_base.go_pose(fridge_pose)
def _invert_pose(pose):
"""Invert a given pose as if it is a transformation.
Args:
pose (geometry.Pose): A pose to be inverted.q
Returns:
geometry.Pose: The result of computation
"""
m = T.compose_matrix(translate=pose[0],
angles=T.euler_from_quaternion(pose[1]))
(_, _, euler, trans, _) = T.decompose_matrix(T.inverse_matrix(m))
q = T.quaternion_from_euler(euler[0], euler[1], euler[2])
return geometry.Pose(trans, q)
def _pose_from_x_axis(axis):
"""Compute a transformation that fits X-axis of its frame to given vector.
Args:
axis (geometry.Vector3): A target vector
Returns:
geometry.Pose: The result transformation that stored in Pose type.
"""
axis = np.array(axis, dtype='float64', copy=True)
axis = _normalize_np(axis)
unit_x = np.array([1, 0, 0])
outerp = np.cross(unit_x, axis)
theta = math.acos(np.dot(unit_x, axis))
if np.linalg.norm(outerp) < sys.float_info.epsilon:
outerp = np.array([0, 1, 0])
outerp = _normalize_np(outerp)
q = T.quaternion_about_axis(theta, outerp)
return geometry.Pose(geometry.Vector3(0, 0, 0), geometry.Quaternion(*q))
def _plan_robot_action(self,
action,
odom_to_robot_pose,
initial_joint_state,
collision_env=None):
if action.type == Action.POSE:
x = action.odom_to_hand_pose.position.x
y = action.odom_to_hand_pose.position.y
z = action.odom_to_hand_pose.position.z
vec3 = geometry.Vector3(*(x, y, z))
x = action.odom_to_hand_pose.orientation.x
y = action.odom_to_hand_pose.orientation.y
z = action.odom_to_hand_pose.orientation.z
w = action.odom_to_hand_pose.orientation.w
quaternion = geometry.Quaternion(*(x, y, z, w))
pose = geometry.Pose(vec3, quaternion)
return self._move_end_effector_pose(pose, odom_to_robot_pose,
initial_joint_state,
collision_env)
elif action.type == Action.LINE:
x = action.axis.x
y = action.axis.y
z = action.axis.z
axis = geometry.Vector3(*(x, y, z))
return self._move_end_effector_by_line(axis, action.distance,
odom_to_robot_pose,
initial_joint_state,
collision_env)
elif action.type == Action.GRIPPER:
return self._command_gripper(action.open_angle, action.motion_time)
else:
raise Exception("type must be pose, line, or gripper")
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
