def apply(self, pose):
if self.prev_pose == None:
self.prev_pose = pose.copy()
return pose
self.prev_pose = RigidTransform.interpolate(self.prev_pose, pose, self.alpha)
return self.prev_pose.copy()
def view_segmentation(mesh, n_segs):
seg_graph = SegmentGraph(FaceGraph(mesh))
seg_graph.cut_to_k_segments(n_segs)
seg_graph.reindex_segment_nodes()
seg_graph.refine_all_edges(0.4)
Visualizer3D.figure(size=(100, 100))
for i, seg_node in enumerate(seg_graph.segment_nodes):
segment = Segment(seg_node, mesh)
Visualizer3D.mesh(segment.mesh,
style='surface',
color=indexed_color(i))
Visualizer3D.show()
seg_map = {}
n_segs -= 1
while n_segs > 2:
n_segs -= 1
seg_graph.cut_to_k_segments(n_segs)
for seg_node in seg_graph.segment_nodes:
if (frozenset(seg_node.segment_indices) not in seg_map):
segment = Segment(seg_node, mesh)
seg_map[frozenset(seg_node.segment_indices)] = segment
for i, segment in enumerate(
sorted(seg_map.values(), key=lambda x: -x.cut_cost / x.perimeter)):
print segment.cut_cost / segment.perimeter
Visualizer3D.mesh(
segment.mesh,
style='surface',
color=indexed_color(i),
T_mesh_world=RigidTransform(translation=[0.15, 0, 0]))
Visualizer3D.show()
def get_pose(self, raw_res=False):
'''Get the current pose of this arm to base frame of the arm.
Parameters
----------
raw_res : bool, optional
If True, will return raw_res namedtuple instead of YuMiState
Defaults to False
Returns
-------
out :
RigidTransform if raw_res is False
_RES(raw_res, pose) namedtuple if raw_res is True
Raises
------
YuMiCommException
If communication times out or socket error.
'''
if self._debug:
return RigidTransform(from_frame=self._from_frame,
to_frame=self._to_frame)
req = YuMiArm._construct_req('get_pose')
res = self._request(req, True)
if res is not None:
pose = message_to_pose(res.message, self._from_frame)
if raw_res:
return _RES(res, pose)
else:
return pose
def message_to_pose(message, from_frame='yumi'):
tokens = message.split()
try:
if len(tokens) != 7:
raise Exception(
"Invalid format for pose! Got:\n{0}".format(message))
pose_vals = [float(token) for token in tokens]
q = pose_vals[3:]
t = pose_vals[:3]
R = RigidTransform.rotation_from_quaternion(q)
pose = RigidTransform(R, t, from_frame=from_frame)
pose.position = pose.position * MM_TO_METERS
return pose
except Exception, e:
logging.error(e)
def apply(self, pose):
if self.prev_pose == None:
self.prev_pose = pose.copy()
return pose
self.prev_pose = RigidTransform.interpolate(self.prev_pose, pose,
self.alpha)
return self.prev_pose.copy()
def _reset_init_poses(self, yumi_pose):
rospy.loginfo("Reset Init Pose for {} at {}".format(
self.pub_name,
time() - START_TIME))
rospy.loginfo('Received yumi pose for {} is {}'.format(
self.pub_name, yumi_pose.translation))
self.cb_prop_q.put({'has_zeroed': False})
self.T_mz_cu_t = RigidTransform(from_frame=self._clutch('up'),
to_frame='masters_zero')
self.T_w_cu_t = self.T_w_mc.as_frames(self._clutch('up'), 'world')
self.T_mzr_mz = RigidTransform(rotation=self.T_w_cu_t.rotation,
from_frame='masters_zero',
to_frame='masters_zero_ref')
self.T_mc_mcr = RigidTransform(
rotation=self.T_w_cu_t.inverse().rotation,
from_frame='masters_current_ref',
to_frame='masters_current')
self.T_w_yi = yumi_pose.copy()
self.T_yi_yir = RigidTransform(rotation=self.T_w_yi.inverse().rotation,
from_frame='yumi_init_ref',
to_frame='yumi_init')
self.T_ycr_yc = RigidTransform(rotation=self.T_w_yi.rotation,
from_frame='yumi_current',
to_frame='yumi_current_ref')
self.cb_prop_q.put({
'T_w_cu_t': self.T_w_cu_t,
'T_mzr_mz': self.T_mzr_mz,
'T_mc_mcr': self.T_mc_mcr,
'T_w_yi': self.T_w_yi,
'T_yi_yir': self.T_yi_yir,
'T_ycr_yc': self.T_ycr_yc
})
self.cb_prop_q.put({'has_zeroed': True})
rospy.loginfo("Done Init Pose for {} at {}".format(
self.pub_name,
time() - START_TIME))
def ros_pose_to_T(ros_pose, from_frame, to_frame):
translation = [
ros_pose.position.x, ros_pose.position.y, ros_pose.position.z
]
rotation = [
ros_pose.orientation.w, ros_pose.orientation.x, ros_pose.orientation.y,
ros_pose.orientation.z
]
T = RigidTransform(rotation=rotation,
translation=translation,
from_frame=from_frame,
to_frame=to_frame)
return T
def contacts_to_baxter_hand_pose(contact1, contact2):
c1 = np.array(contact1)
c2 = np.array(contact2)
# compute gripper center and axis
center = 0.5 * (c1 + c2)
y_axis = c2 - c1
y_axis = y_axis / np.linalg.norm(y_axis)
z_axis = np.array([y_axis[1], -y_axis[0], 0]) # the z axis will always be in the table plane for now
z_axis = z_axis / np.linalg.norm(z_axis)
x_axis = np.cross(y_axis, z_axis)
# convert to hand pose
R_obj_gripper = np.array([x_axis, y_axis, z_axis]).T
t_obj_gripper = center
return RigidTransform(rotation=R_obj_gripper,
translation=t_obj_gripper,
from_frame='gripper',
to_frame='obj')
def _clutch_callback(self, msg):
if not self.has_zeroed:
return
clutch_down = msg.data
if clutch_down:
self._clutch_i += 1
rospy.loginfo("Got clutch down: {0}".format(self._clutch_i))
# clutch down
self.T_w_cd_t1 = self.T_w_mc.as_frames(self._clutch('down'),
'world')
else:
# clutch up
rospy.loginfo("Updating clutch up for {0}".format(self._clutch_i))
self.T_mz_cu_t = self.T_mz_cu_t * self.T_w_cu_t.inverse() * \
self.T_w_cd_t1 * RigidTransform(from_frame=self._clutch('up'), to_frame=self._clutch('down'))
# updating last known clutch up pose
self.T_w_cu_t = self.T_w_mc.as_frames(self._clutch('up'), 'world')
self.clutch_state = clutch_down
target poses for the YuMi.
Meant to operate along yumi_teleop_client and yumi_teleop_host
Author: Jacky Liang
"""
import rospy, os
from std_msgs.msg import Bool
from geometry_msgs.msg import Pose
from time import time
from masters_control.srv import pose_str
import numpy as np
from core import RigidTransform
from yumi_teleop import T_to_ros_pose, ros_pose_to_T
from multiprocessing import Queue
_T_MC_YCR = RigidTransform(rotation=[[0, -1, 0], [1, 0, 0], [0, 0, 1]],
from_frame='yumi_current_ref',
to_frame='masters_current')
_T_YIR_MI = RigidTransform(rotation=[[0, 1, 0], [-1, 0, 0], [0, 0, 1]],
from_frame='masters_init',
to_frame='yumi_init_ref')
START_TIME = time()
class MastersYuMiConnector:
def __init__(self, name):
self.clutch_state = False
self._clutch_i = 0
# masters reference poses
def __init__(self, name, ar_marker, R_ar_obj=np.eye(3), t_ar_obj=np.zeros(3)):
self.ar_marker = ar_marker
self.T_ar_obj = RigidTransform(rotation=R_ar_obj, translation=t_ar_obj,
from_frame=name, to_frame=ar_marker)
class YuMiConstants:
IP = '192.168.125.1'
PORTS = {
"left": {
"server": 5000,
"states": 5010,
"poses": 5012,
"torques": 5014
},
"right": {
"server": 5001,
"states": 5011,
"poses": 5013,
"torques": 5015
},
}
BUFSIZE = 4096
MOTION_TIMEOUT = 8
COMM_TIMEOUT = 5
PROCESS_TIMEOUT = 10
PROCESS_SLEEP_TIME = 0.01
GRASP_COUNTER_PATH = '/home/autolab/Public/alan/grasp_counts'
# used to rate limit real-time YuMi controls (in seconds)
COMM_PERIOD = 0.04
DEBUG = False
LOGGING_LEVEL = logging.DEBUG
# reset mechanism
RESET_RIGHT_COMM = '/dev/ttyACM0'
RESET_BAUDRATE = 115200
CMD_CODES = {
'ping': 0,
'goto_pose_linear': 1,
'goto_joints': 2,
'get_pose': 3,
'get_joints': 4,
'goto_pose': 5,
'set_tool': 6,
'set_speed': 8,
'set_zone': 9,
'goto_pose_sync': 11,
'goto_joints_sync': 12,
'goto_pose_delta': 13,
'close_gripper': 20,
'open_gripper': 21,
'calibrate_gripper': 22,
'set_gripper_max_speed': 23,
'set_gripper_force': 24,
'move_gripper': 25,
'get_gripper_width': 26,
'set_circ_point': 35,
'move_by_circ_point': 36,
'buffer_add': 30,
'buffer_clear': 31,
'buffer_size': 32,
'buffer_move': 33,
'is_pose_reachable': 40,
'is_joints_reachable': 41,
'close_connection': 99,
'reset_home': 100,
}
RES_CODES = {'failure': 0, 'success': 1}
SUB_CODES = {'pose': 0, 'state': 1}
MOVEIT_PLANNER_IDS = {
'SBL': 'SBLkConfigDefault',
'EST': 'ESTkConfigDefault',
'LBKPIECE': 'LBKPIECEkConfigDefault',
'BKPIECE': 'BKPIECEkConfigDefault',
'KPIECE': 'KPIECEkConfigDefault',
'RRT': 'RRTkConfigDefault',
'RRTConnect': 'RRTConnectkConfigDefault',
'RRTstar': 'RRTstarkConfigDefault',
'TRRT': 'TRRTkConfigDefault',
'PRM': 'PRMkConfigDefault',
'PRMstar': 'PRMstarkConfigDefault'
}
MOVEIT_PLANNING_REFERENCE_FRAME = 'yumi_body'
ROS_TIMEOUT = 10
T_GRIPPER_HAND = RigidTransform(translation=[0, 0, -0.157],
from_frame='gripper',
to_frame='gripper')
TCP_ABB_GRIPPER = RigidTransform(translation=[0, 0, 0.136])
TCP_ABB_GRASP_GRIPPER = RigidTransform(translation=[0, 0, 0.136 - 0.0065])
TCP_LONG_GRIPPER = RigidTransform(
translation=[0, 0, (136 - 56 + 88 - 12) / 1000.])
TCP_DEFAULT_GRIPPER = RigidTransform(
translation=[0, 0, (136 - 56 + 88 - 12) / 1000.])
L_HOME_STATE = YuMiState([0, -130, 30, 0, 40, 0, 135])
L_HOME_POSE = RigidTransform(
translation=[0.123, 0.147, 0.124],
rotation=[0.06551, 0.84892, -0.11147, 0.51246])
R_HOME_STATE = YuMiState([0, -130, 30, 0, 40, 0, -135])
R_HOME_POSE = RigidTransform(
translation=[-0.0101, -0.1816, 0.19775],
rotation=[-0.52426, 0.06481, -0.84133, -0.11456])
R_FORWARD_STATE = YuMiState(
[9.66, -133.36, 34.69, -13.19, 28.85, 28.81, -110.18])
R_FORWARD_POSE = RigidTransform(
translation=[0.07058, -0.26519, 0.19775],
rotation=[-0.52425, 0.0648, -0.84133, -0.11454])
R_AWAY_POSE = RigidTransform(translation=[0.15, -0.4, 0.22],
rotation=[0.38572, 0.39318, 0.60945, 0.57027])
L_AWAY_POSE = RigidTransform(
translation=[0.30, 0.12, 0.16],
rotation=[0.21353, -0.37697, 0.78321, -0.44596])
AXIS_ALIGNED_STATES = {
'inwards': {
'right': YuMiState([75, -90, 0, -30, 90, -10, -45]),
'left': YuMiState([-75, -90, 0, 30, 90, 10, 45])
},
'forwards': {
'right':
YuMiState([36.42, -117.3, 35.59, 50.42, 46.19, 66.02, -100.28]),
'left':
YuMiState([-36.42, -117.3, 35.59, -50.42, 46.19, 113.98, 100.28])
},
'downwards': {
'right':
YuMiState([-60.0, -110.0, 35.0, 116.0, 100.0, -90.0, 30.0]),
'left':
YuMiState([60.0, -110.0, 35.0, -116.0, 100.0, -90.0, -30.0])
}
}
L_THINKING_POSES = [
RigidTransform(translation=[0.32, 0.12, 0.16],
rotation=[0.21353, -0.37697, 0.78321, -0.44596]),
RigidTransform(translation=[0.30, 0.10, 0.16],
rotation=[0.21353, -0.37697, 0.78321, -0.44596]),
RigidTransform(translation=[0.28, 0.12, 0.16],
rotation=[0.21353, -0.37697, 0.78321, -0.44596]),
RigidTransform(translation=[0.30, 0.14, 0.16],
rotation=[0.21353, -0.37697, 0.78321, -0.44596])
]
R_READY_STATE = YuMiState(
[51.16, -99.4, 21.57, -107.19, 84.11, 94.61, -36.00])
L_READY_STATE = YuMiState(
[-51.16, -99.4, 21.57, 107.19, 84.11, 94.61, 36.00])
L_PREGRASP_POSE = RigidTransform(
translation=[0.30, 0.20, 0.16],
rotation=[0.21353, -0.37697, 0.78321, -0.44596])
L_KINEMATIC_AVOIDANCE_POSE = RigidTransform(
translation=[0.45, -0.05, 0.15], rotation=[0, 0, 1, 0])
L_RAISED_STATE = YuMiState(
[5.5, -99.46, 20.52, -21.03, 67, -22.31, 110.11])
L_RAISED_POSE = RigidTransform(
translation=[-0.0073, 0.39902, 0.31828],
rotation=[0.54882, 0.07398, 0.82585, -0.10630])
L_FORWARD_STATE = YuMiState(
[-21.71, -142.45, 43.82, 31.14, 10.43, -40.67, 106.63])
L_FORWARD_POSE = RigidTransform(
translation=[0.13885, 0.21543, 0.19774],
rotation=[0.55491, 0.07064, 0.82274, -0.1009])
L_PREDROP_POSE = RigidTransform(
translation=[0.55, 0.0, 0.20],
rotation=[0.09815, -0.20528, 0.97156, -0.06565])
L_PACKAGE_DROP_POSES = [
RigidTransform(translation=[0.33, 0.42, 0.25],
rotation=[0.09078, -0.31101, 0.91820, -0.22790]),
RigidTransform(translation=[0.33, 0.33, 0.25],
rotation=[0.09078, -0.31101, 0.91820, -0.22790]),
RigidTransform(translation=[0.23, 0.33, 0.25],
rotation=[0.09078, -0.31101, 0.91820, -0.22790]),
RigidTransform(translation=[0.23, 0.42, 0.25],
rotation=[0.09078, -0.31101, 0.91820, -0.22790])
]
L_REJECT_DROP_POSES = [
RigidTransform(translation=[0.60, 0.42, 0.23],
rotation=[0.09078, -0.31101, 0.91820, -0.22790]),
RigidTransform(translation=[0.60, 0.35, 0.23],
rotation=[0.09078, -0.31101, 0.91820, -0.22790]),
RigidTransform(translation=[0.54, 0.35, 0.23],
rotation=[0.09078, -0.31101, 0.91820, -0.22790]),
RigidTransform(translation=[0.54, 0.42, 0.23],
rotation=[0.09078, -0.31101, 0.91820, -0.22790])
]
BOX_CLOSE_SEQUENCE = [
#('L', RigidTransform(translation=[0.071, 0.385, 0.118], rotation=[0.06744, 0.84050, -0.11086, 0.52604])),
#('L', RigidTransform(translation=[0.213, 0.385, 0.118], rotation=[0.06744, 0.84050, -0.11086, 0.52604])),
#('L', RigidTransform(translation=[0.213, 0.385, 0.156], rotation=[0.06744, 0.84050, -0.11086, 0.52604])),
#('L', RigidTransform(translation=[0.323, 0.385, 0.156], rotation=[0.06744, 0.84050, -0.11086, 0.52604])),
#('L', RigidTransform(translation=[0.323, 0.385, 0.144], rotation=[0.06744, 0.84050, -0.11086, 0.52604])),
('R', [0, 0.300, 0]),
('R', [0.100, 0, 0]),
('R',
YuMiState([111.30, -65.37, -29.93, -50.53, 65.41, -94.59, -68.84])),
('R', YuMiState([147.13, -76.28, -58.98, -13.85, 54.95, 54.95,
-96.95])),
('R', [0, 0, 0.070]),
('R', [-0.074, 0, 0]),
('R', [0, -0.012, 0]),
('R', [0, 0, -0.040]),
('R', YuMiState([132.32, -56.98, -8.13, -72.42, 36.44, 135.16,
-99.36])),
('R',
YuMiState([146.72, -57.09, -9.44, -101.74, 69.26, 155.98, -98.41])),
('R',
YuMiState([168.36, -64.12, -23.60, -118.0, 97.50, 163.97, -102.95])),
('R',
YuMiState([168.37, -59.60, 2.63, -146.15, 95.29, 199.63, -129.70])),
('R',
YuMiState([168.37, -82.24, -10.81, -196.49, 115.92, 199.63,
-148.06])),
('L',
RigidTransform(translation=[0.323, 0.385, 0.203],
rotation=[0.06744, 0.84050, -0.11086, 0.52604])),
('L',
RigidTransform(translation=[0.203, 0.478, 0.203],
rotation=[0.06744, 0.84050, -0.11086, 0.52604])),
('L',
RigidTransform(translation=[0.203, 0.478, 0.092],
rotation=[0.06744, 0.84050, -0.11086, 0.52604])),
('L',
RigidTransform(translation=[0.305, 0.478, 0.092],
rotation=[0.06744, 0.84050, -0.11086, 0.52604])),
('L',
RigidTransform(translation=[0.305, 0.478, 0.183],
rotation=[0.06744, 0.84050, -0.11086, 0.52604])),
('L',
RigidTransform(translation=[0.305, 0.359, 0.183],
rotation=[0.06744, 0.84050, -0.11086, 0.52604])),
('L',
RigidTransform(translation=[0.305, 0.359, 0.149],
rotation=[0.06744, 0.84050, -0.11086, 0.52604])),
('L',
RigidTransform(translation=[0.425, 0.359, 0.149],
rotation=[0.06744, 0.84050, -0.11086, 0.52604])),
('L',
RigidTransform(translation=[0.425, 0.359, 0.245],
rotation=[0.06744, 0.84050, -0.11086, 0.52604])),
('L',
RigidTransform(translation=[0.265, 0.445, 0.245],
rotation=[0.06744, 0.84050, -0.11086, 0.52604]))
]
"""
from matplotlib import pyplot as plt
from perception.object_render import RenderMode
dexnet_path = "/home/chris/optimal_manipulation_simulator/dex-net-new-api"
if __name__ == "__main__":
print "\n\n\n\n"
camera_intr = CameraIntrinsics.load("../config/primesense_overhead.intr")
camera = VirtualCamera(camera_intr)
of = ObjFile(dexnet_path + "/data/meshes/chess_pieces/WizRook.obj")
rook1 = of.read()
T_world_camera = RigidTransform(rotation=np.array([[-1, 0, 0], [0, 1, 0],
[0, 0, -1]]),
translation=np.array([0, 0, .2]).T,
from_frame="world",
to_frame="primesense_overhead")
# T_world_camera = RigidTransform(rotation = np.array([[1,0,0],[0,1,0],[0,0,1]]),
# translation=np.array([-.2,0,0]).T,
# from_frame="world",
# to_frame="primesense_overhead")
# squares = [(-1,-1), (-1,0), (-1,1), (0,-1), (0,1), (1,-1), (1,0), (1,1)]
# for i, square in enumerate(squares):
T_rook2_world = RigidTransform(rotation=np.eye(3),
translation=np.array([.06, .06, 0]).T,
from_frame="rook2",
to_frame="obj")
of2 = ObjFile(dexnet_path + "/data/meshes/chess_pieces/WizRook.obj")
def load_registration_tf(trial_path, device):
if device not in ('webcam', 'primesense'):
raise ValueError("Can only accept webcam or primesense. Got {}".format(device))
return RigidTransform.load(os.path.join(trial_path, '{}_overhead_to_world.tf'.format(device)))