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