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 _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 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 _movement_axis_and_distance(pose1, pose2):
"""Compute a vector from the origin of pose1 to pose2 and distance.
Args:
pose1 (geometry.Pose): A pose that its origin is used as start.
pose2 (geometry.Pose): A pose that its origin is used as goal.
Returns:
Tuple[geometry.Vector3, float]: The result
"""
p1 = pose1[0]
p2 = pose2[0]
"""Normalize Vector3"""
x = p2[0] - p1[0]
y = p2[1] - p1[1]
z = p2[2] - p1[2]
length = math.sqrt(x * x + y * y + z * z)
if length < sys.float_info.epsilon:
return geometry.Vector3(0.0, 0.0, 0.0), 0.0
else:
x /= length
y /= length
z /= length
return geometry.Vector3(x, y, z), length
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 step(self, action):
try:
# Big move
if action == 0:
self._move(geometry.Vector3(1, 0, 0), 0.1)
elif action == 1:
self._move(geometry.Vector3(-1, 0, 0), 0.1)
elif action == 2:
self._move(geometry.Vector3(0, 1, 0), 0.1)
elif action == 3:
self._move(geometry.Vector3(0, -1, 0), 0.1)
elif action == 4:
self._move(geometry.Vector3(0, 0, 1), 0.1)
elif action == 5:
self._move(geometry.Vector3(0, 0, -1), 0.1)
# Small moves
elif action == 6:
self._move(geometry.Vector3(1, 0, 0), 0.01)
elif action == 7:
self._move(geometry.Vector3(-1, 0, 0), 0.01)
elif action == 8:
self._move(geometry.Vector3(0, 1, 0), 0.01)
elif action == 9:
self._move(geometry.Vector3(0, -1, 0), 0.01)
elif action == 10:
self._move(geometry.Vector3(0, 0, 1), 0.01)
elif action == 11:
self._move(geometry.Vector3(0, 0, -1), 0.01)
# Gripper
elif action == 12:
self._gripper_command(1.2) # Open
elif action == 13:
self._gripper_command(0.0) # Close
elif action == 14:
self._gripper_apply_force(0.5)
elif action == 15:
self._gripper_apply_force(1.0)
else:
raise NotImplementedError
except Exception as e:
print("Action %d is unavailable: %s" % (action, e))
observation = self.state
reward = self._reward()
done = False if reward < 0.9 else True
info = None
return observation, reward, done, info
#!/usr/bin/env python
import rospy
from hsrb_interface import Robot, exceptions, geometry
from std_srvs.srv import Empty
import math
rospy.loginfo('Getting robot resources')
print('Starting')
robot = None
while not rospy.is_shutdown():
try:
robot = Robot()
whole_body = robot.try_get('whole_body')
omni_base = robot.try_get('omni_base')
print('got resources')
break
except (exceptions.ResourceNotFoundError,
exceptions.RobotConnectionError) as e:
rospy.logerr_throttle(
1, 'Failed to obtain resource: {}\nRetrying...'.format(e))
whole_body.gaze_point(point=geometry.Vector3(x=5, y=5, z=0),
ref_frame_id='map')
whole_body.gaze_point(point=geometry.Vector3(x=-5, y=5, z=0),
ref_frame_id='map')
rospy.sleep(5)
