-
-
Save RicoJia/4a928cd7f669f328ff3a5872e2ff0e51 to your computer and use it in GitHub Desktop.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| import pybullet as p | |
| def apply_external_force(self, force, link_index, position=[0,0,0]): | |
| """ | |
| Apply the specified external force on the specified position on the body / link. | |
| Args: | |
| link_index (int): unique link id. If -1, it will be the base. | |
| force (np.array[float[3]]): external force to be applied. | |
| position (np.array[float[3]], None): position on the link where the force is applied. See `flags` for | |
| coordinate systems. If None, it is the center of mass of the body (or the link if specified). | |
| frame (int): Specify the coordinate system of force/position: either `pybullet.WORL D_FRAME` (=2) for | |
| Cartesian world coordinates or `pybullet.LINK_FRAME` (=1) for local link coordinates. | |
| """ | |
| p.applyExternalForce( | |
| objectUniqueId=self.model_unique_id, linkIndex=link_index, | |
| forceObj=force, posObj=position, flags=p.LINK_FRAME) | |
| #This is essential, as this disables the default motor imposed by bullet. These default motors prevent overspeeding, and functions as | |
| #friction drives. | |
| p.setJointMotorControl2(self.model_unique_id, self.secondary_joint_id, | |
| controlMode=p.VELOCITY_CONTROL, force=0) | |
| spring_angle = np.pi/2.0 | |
| TORQUE_K = 10.0 | |
| torque = spring_angle * TORQUE_K | |
| #apply external force | |
| self.apply_external_force(force= torque * self.scaled_force_axis_c, link_index=self.child_link_id, position=self.force_pos_c) | |
| self.apply_external_force(force= torque * self.scaled_force_axis_p, link_index=self.parent_link_id, position=self.force_pos_p) | |
| # print("torque: ", torque) | |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment