def reset(self, T_world_tcp):
T_world_body = T_world_tcp * self.T_tcp_body
self.body = self.world.load_urdf(self.urdf_path, T_world_body)
self.body.set_pose(
T_world_body) # sets the position of the COM, not URDF link
self.constraint = self.world.add_constraint(
self.body,
None,
None,
None,
pybullet.JOINT_FIXED,
[0.0, 0.0, 0.0],
Transform.identity(),
T_world_body,
)
self.update_tcp_constraint(T_world_tcp)
# constraint to keep fingers centered
self.world.add_constraint(
self.body,
self.body.links["panda_leftfinger"],
self.body,
self.body.links["panda_rightfinger"],
pybullet.JOINT_GEAR,
[1.0, 0.0, 0.0],
Transform.identity(),
Transform.identity(),
).change(gearRatio=-1, erp=0.1, maxForce=50)
self.joint1 = self.body.joints["panda_finger_joint1"]
self.joint1.set_position(0.5 * self.max_opening_width, kinematics=True)
self.joint2 = self.body.joints["panda_finger_joint2"]
self.joint2.set_position(0.5 * self.max_opening_width, kinematics=True)
def draw_workspace(size):
scale = size * 0.005
pose = Transform.identity()
scale = [scale, 0.0, 0.0]
color = [0.5, 0.5, 0.5]
msg = _create_marker_msg(Marker.LINE_LIST, "task", pose, scale, color)
msg.points = [
ros_utils.to_point_msg(point) for point in workspace_lines(size)
]
pubs["workspace"].publish(msg)