Simulate a rope for ball in the cup task

Official Python bindings with a focus on reinforcement learning and robotics.
Post Reply
Silversircel
Posts: 1
Joined: Sat Jul 18, 2020 11:51 am

Simulate a rope for ball in the cup task

Post by Silversircel »

Hello,

I am trying to simulate a rope for a ball in the cup scenario, using pyBullet.

Since I am fairly new to pyBullet, I started by simply connecting n spherical joints in a chain, each linked by a small cylinder.

However, I don't manage to tune the friction in these joints correctly to prevent the simulation from 'exploding'.
I've tested:
- Setting 'setJointMotorControlMultiDof' to p.POSITION_CONTROL, with a desired Quaternion of [0,0,0,1] and a omega of [0,0,0].
Then adding a small force [fx, fy, fz] to simulate joint friction.
- Setting 'setJointMotorControlMultiDof' to p.TORQUE_CONTROL and adding a damping torque = -Damping*omega,
where omega is the velocity obtained from p.getJointStateMultiDof

If I increase friction coefficients I can sometimes get rid of the instability, but then the chain of cylinders stops behaving like a rope.
Here is a short example code:

Code: Select all

import pybullet as p
import time
import numpy as np
from numpy import linalg as la
import math

p.connect(p.GUI)
p.createCollisionShape(p.GEOM_PLANE)
p.createMultiBody(0,0)

# PARAMETERS
n_links = 30          # num links
dx_link = 0.02        # length of link segment
link_mass = 0.005     # 5g
base_mass = 0.1       # 100g

joint_friction = 0.0005  # rotational joint friction [N/(rad/s)]

# setup shapes
link_shape = p.createCollisionShape(p.GEOM_CYLINDER, 
                                    radius=0.005, 
                                    height=dx_link, 
                                    collisionFramePosition=[0, 0, -dx_link/2])
base_shape = p.createCollisionShape(p.GEOM_BOX,
                                    halfExtents=[0.01, 0.01, 0.01])

linkMasses = [link_mass]*n_links
linkCollisionShapeIndices=[link_shape]*n_links
linkVisualShapeIndices=[-1]*n_links

# relative position 
linkPositions = []
for i in range(n_links):
    linkPositions.append([0,0,-dx_link])

# cm positions
linkOrientations=[[0,0,0,1]]*n_links
linkInertialFramePositions=[[0,0,0]]*n_links
linkInertialFrameOrientations=[[0,0,0,1]]*n_links

# connection graph
indices = range(n_links)

# use spherical joints
jointTypes = [p.JOINT_SPHERICAL]*n_links
jointTypes[1] = p.JOINT_FIXED

# rotational axis (dosnt't matter, spherical)
axis = [[1,0,0]]*n_links

# create rope body
visualShapeId = -1
basePosition = [0,0,2]
baseOrientation = [0,0,0,1]
rope = p.createMultiBody(base_mass,base_shape,visualShapeId,basePosition,baseOrientation,
                        linkMasses=linkMasses,
                        linkCollisionShapeIndices=linkCollisionShapeIndices,
                        linkVisualShapeIndices=linkVisualShapeIndices,
                        linkPositions=linkPositions,
                        linkOrientations=linkOrientations,
                        linkInertialFramePositions=linkInertialFramePositions,
                        linkInertialFrameOrientations=linkInertialFrameOrientations,
                        linkParentIndices=indices,
                        linkJointTypes=jointTypes,
                        linkJointAxis=axis)
                        #flags=p.URDF_USE_SELF_COLLISION)


n_joints = p.getNumJoints(rope)

# remove stiffness in motors, add friction force

friction_vec = [joint_friction]*3   # same all axis
control_mode = p.POSITION_CONTROL   # set pos control mode
for j in range(n_joints):
  p.setJointMotorControlMultiDof(rope,j,control_mode,
                                 targetPosition=[0,0,0,1],
                                 targetVelocity=[0,0,0],
                                 positionGain=0,
                                 velocityGain=1,
                                 force=friction_vec)


p.setGravity(0,0,-9.81)

# fixed constrain to keep root cube in place
root_robe_c = p.createConstraint(rope, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0], [0, 0, 2])

# some traj to inject motion
amplitude_x = 0.3
amplitude_y = 0.0
freq = 0.6

# manually simulate joint damping
Damping = 0.001

t = 0.0
freq_sim = 240
while 1:
  time.sleep(1./freq_sim)
  t += 1./freq_sim

  # some trajectory
  ux = amplitude_x*math.sin(2*math.pi*freq*t)
  uy = amplitude_y*math.cos(2*math.pi*freq*t)
  
  # move base arround
  pivot = [ux, uy, 2]
  orn = p.getQuaternionFromEuler([0, 0, 0])
  p.changeConstraint(root_robe_c, pivot, jointChildFrameOrientation=orn, maxForce=500)

  # manually apply viscous friction: f_damping = -Damping*omega
  '''
  for j in range(n_joints):
    q_state = p.getJointStateMultiDof(rope, j)
    omega = q_state[1]
    f_dampling = [-Damping*v for v in omega]
    p.setJointMotorControlMultiDof(rope, j, p.TORQUE_CONTROL, force=f_dampling)
  '''

  # update
  p.stepSimulation()
Is there a better way to simulate stable ropes? Probably switching to bullet instead? Or am I missing some other important parameters that need to be tuned in spherical joints (e.g. in 'changeDynamics')?

Thank you very much

Best Simon
User avatar
Erwin Coumans
Site Admin
Posts: 4232
Joined: Sun Jun 26, 2005 6:43 pm
Location: California, USA
Contact:

Re: Simulate a rope for ball in the cup task

Post by Erwin Coumans »

You can use maximal coordinates for the rope, it is likely more stable. (loadURDF(..., useMaximalCoordinates=True)
Or use deformable bodies, see https://github.com/bulletphysics/bullet ... _anchor.py
Post Reply