Now we want to change the size of the peg (the tool in the gripper) for every reset. The way we tried this by removing the thing from urdf and dynamically attaching it to the robot with this code:
Code: Select all
width = 0.025
depth = 0.025
height = 0.085
csId = self._p.createCollisionShape(shapeType=pybullet.GEOM_BOX,
halfExtents=[width, depth, height],
)
vsId = self._p.createVisualShape(shapeType=pybullet.GEOM_BOX,
halfExtents=[width, depth, height])
pegId = self._p.createMultiBody(baseMass=0.01,
baseCollisionShapeIndex=csId,
baseVisualShapeIndex=vsId,
flags=pybullet.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS,)
)
constId = self._p.createConstraint(parentBodyUniqueId=self.robot_uid,
parentLinkIndex=self._motor_indices[-1],
childBodyUniqueId=pegId,
childLinkIndex=-1,
jointType=pybullet.JOINT_FIXED,
jointAxis=[0, 0, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0.1, 0.1, 0.3])
self._p.changeConstraint(userConstraintUniqueId=constId,
maxForce=9e20,
erp=1e-20)
Does anybody have an idea how to attacht the thing completely rigidly?