def setup_robot(self): if not self.handonly: #self.robot = p.loadURDF(os.environ["HOME"]+"/ros/src/franka_ros/franka_description/robots/model.urdf") #fixme, point somewhere less fragile self.robot = p.loadURDF("../../models/robots/model.urdf") set_point(self.robot, (0, 0, 0.01)) start_joints = (0.09186411075857098, 0.02008522792588543, 0.03645461729775788, -1.9220854528910314, 0.213232566443952983, 1.647271913704007, 0.0, 0.0, 0.0) start_joints = [ 2.78892560e-04, -7.85089406e-01, 4.81729135e-05, -2.35613802e+00, 4.95981896e-04, 1.57082514e+00, 7.85833531e-01, 0, 0 ] self.grasp_joint = 10 p.changeDynamics(self.robot, -1, mass=0) ut.set_joint_positions(self.robot, ut.get_movable_joints(self.robot), start_joints) else: self.robot = p.loadURDF( os.environ["HOME"] + "/ros/src/franka_ros/franka_description/robots/hand.urdf" ) #fixme, point somewhere less fragile init_pos = (0, 0, 0.35) init_quat = (1, 0, 0, 0) self.grasp_joint = 0 #kinda sketchy tbh, try to just use the whole robot import ipdb ipdb.set_trace() ut.set_joint_position(self.robot, 9, 0.03) ut.set_joint_position(self.robot, 10, 0.03) set_pose(self.robot, (init_pos, init_quat))
def fk_rigidtransform(self, joint_vals, return_rt=True): state = p.saveState() ut.set_joint_positions( self.robot, ut.get_movable_joints(self.robot)[:len(joint_vals)], joint_vals) link_trans, link_quat = ut.get_link_pose(self.robot, self.grasp_joint) p.restoreState(state) if return_rt: link_trans = list(link_trans) link_trans[-1] -= 0.055 #conversion to franka_tool link_quat = np.hstack([link_quat[-1], link_quat[0:3]]) rt = RigidTransform(translation=link_trans, rotation=Quaternion(link_quat).rotation_matrix) return rt else: return (link_trans, link_quat)
def get_push_down_traj(self, traj, shape_class=Rectangle): state = p.saveState() #Move in that last bit joint_vals = traj[-1] ut.set_joint_positions( self.robot, ut.get_movable_joints(self.robot)[:len(joint_vals)], joint_vals) goal_pose = self.fk_rigidtransform(traj[-1], return_rt=False) new_trans = (goal_pose[0][0], goal_pose[0][1], goal_pose[0][2] - 1 * self.hole_depth) end_traj = self.make_traj((new_trans, goal_pose[1]), shape_class=shape_class, in_board=True) if end_traj is None: import ipdb ipdb.set_trace() print("No push down") return traj p.restoreState(state) traj = np.vstack([traj, end_traj]) return traj
def setup_robot(self): if not self.handonly: #self.robot = p.loadURDF(os.environ["HOME"]+"/ros/src/franka_ros/franka_description/robots/model.urdf") #fixme, point somewhere less fragile self.robot = p.loadURDF("../../models/robots/model.urdf") set_point(self.robot, (0, 0, 0.01)) start_joints = [ 2.28650215, -1.36063288, -1.4431576, -1.93011263, 0.23962597, 2.6992652, 0.82820212, 0.0, 0.0 ] self.grasp_joint = 8 p.changeDynamics(self.robot, -1, mass=0) ut.set_joint_positions(self.robot, ut.get_movable_joints(self.robot), start_joints) else: self.robot = p.loadURDF( os.environ["HOME"] + "/ros/src/franka_ros/franka_description/robots/hand.urdf" ) #fixme, point somewhere less fragile init_pos = (0, 0, 0.35) init_quat = (1, 0, 0, 0) self.grasp_joint = 0 #kinda sketchy tbh, try to just use the whole robot set_pose(self.robot, (init_pos, init_quat))
def set_joints(self, joint_vals): ut.set_joint_positions( self.robot, ut.get_movable_joints(self.robot)[:len(joint_vals)], joint_vals) for attachment in self.in_hand: attachment.assign()