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