Skip to content

Instantly share code, notes, and snippets.

@RicoJia
Created December 11, 2020 22:46
Show Gist options
  • Select an option

  • Save RicoJia/4a928cd7f669f328ff3a5872e2ff0e51 to your computer and use it in GitHub Desktop.

Select an option

Save RicoJia/4a928cd7f669f328ff3a5872e2ff0e51 to your computer and use it in GitHub Desktop.
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