Skip to content

Commit

Permalink
Merge branch 'main' of github.com:contactrika/dedo into main
Browse files Browse the repository at this point in the history
  • Loading branch information
@yonkshi
yonkshi committed Mar 3, 2022
2 parents c47a0c1 + ec1a1e5 commit 8a0b4dd
Show file tree
Hide file tree
Showing 5 changed files with 63 additions and 56 deletions.
7 changes: 3 additions & 4 deletions README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,8 +16,6 @@ loading custom objects and textures, adjusting material properties.

For a brief overview, please see our intro [video](https://www.youtube.com/watch?v=5eA8z80c9Zc). For more details please see the [paper](https://openreview.net/forum?id=WcY35wjmCBA).

**Note: updates for this repo are in progress (until the presentation at NeurIPS2021 in mid-December).**

```
@inproceedings{dedo2021,
title={Dynamic Environments with Deformable Objects},
Expand Down Expand Up @@ -51,10 +49,11 @@ a direct install without using virtual environments is ok as well.
```
git clone https://github.com/contactrika/dedo
cd dedo
pip install numpy # important: Nessasary for compiling numpy-enabled PyBullet
pip install numpy # important: for numpy-enabled PyBullet
pip install -e .
```
Python3.7 is recommended as we have encountered that on some OS + CPU combo, PyBullet could not be compiled with Numpy enabled in Pip Python 3.8.
Python3.7 is recommended, since PyBullet compilation can have difficulties with Python 3.8 in some cases.

To enable recording/logging videos install ffmpeg:
```
sudo apt-get install ffmpeg
Expand Down
10 changes: 5 additions & 5 deletions dedo/data/robots/fetch/fetch.urdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -92,7 +92,7 @@
<damping value="30000"/>
</contact>
</link>
<joint name="r_wheel_joint" type="continuous">
<joint name="r_wheel_joint" type="fixed">
<origin rpy="-6.123E-17 0 0" xyz="0.0012914 -0.18738 0.055325" />
<parent link="base_link" />
<child link="r_wheel_link" />
Expand Down Expand Up @@ -127,7 +127,7 @@
</contact>

</link>
<joint name="l_wheel_joint" type="continuous">
<joint name="l_wheel_joint" type="fixed">
<origin rpy="-6.123E-17 0 0" xyz="0.0012914 0.18738 0.055325" />
<parent link="base_link" />
<child link="l_wheel_link" />
Expand Down Expand Up @@ -155,7 +155,7 @@
</geometry>
</collision>
</link>
<joint name="torso_lift_joint" type="prismatic">
<joint name="torso_lift_joint" type="fixed">
<origin rpy="-6.123E-17 0 0" xyz="-0.086875 0 0.37743" />
<parent link="base_link" />
<child link="torso_lift_link" />
Expand Down Expand Up @@ -186,7 +186,7 @@
</geometry>
</collision>
</link>
<joint name="head_pan_joint" type="revolute">
<joint name="head_pan_joint" type="fixed">
<origin rpy="0 0 0" xyz="0.053125 0 0.603001417713939" />
<parent link="torso_lift_link" />
<child link="head_pan_link" />
Expand Down Expand Up @@ -214,7 +214,7 @@
</geometry>
</collision>
</link>
<joint name="head_tilt_joint" type="revolute">
<joint name="head_tilt_joint" type="fixed">
<origin rpy="0 0.2617993877991494 0" xyz="0.14253 0 0.057999" />
<parent link="head_pan_link" />
<child link="head_tilt_link" />
Expand Down
4 changes: 2 additions & 2 deletions dedo/envs/deform_robot_env.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -111,9 +111,9 @@ def do_action(self, action, unscaled=False):
tgt_qpos = self.robot.ee_pos_to_qpos(**tgt_kwargs)
n_slack = 1 # use > 1 if robot has trouble reaching the pose
sub_i = 0
ee_th = 0.01
max_diff = 0.02
diff = self.robot.get_qpos() - tgt_qpos
while (diff > ee_th).any():
while (np.abs(diff) > max_diff).any():
self.robot.move_to_qpos(
tgt_qpos, mode=pybullet.POSITION_CONTROL, kp=0.1, kd=1.0)
self.sim.stepSimulation()
Expand Down
84 changes: 46 additions & 38 deletions dedo/utils/bullet_manipulator.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -120,6 +120,18 @@ def __init__(self, sim, robot_desc_file, ee_joint_name, ee_link_name,
assert(len(self.info.left_arm_jids_lst)==len(left_rest_arm_qpos))
self.rest_qpos[self.info.left_arm_jids_lst] = left_rest_arm_qpos[:]
self.reset()
self.default_ik_args = {
'lowerLimits': self.info.joint_minpos.tolist(),
'upperLimits': self.info.joint_maxpos.tolist(),
'jointRanges': (self.info.joint_maxpos -
self.info.joint_minpos).tolist(),
'restPoses': self.rest_qpos.tolist(),
# Note that large num iterations could slow down compute enough
# s.t. visualizer shows differences in rate of following traj.
'maxNumIterations': 500,
'residualThreshold': 0.005,
# solver=pybullet.IK_SDLS,
}
ee_pos, ee_ori, *_ = self.get_ee_pos_ori_vel()
print('Initialized robot ee at pos', ee_pos,
'euler ori', sin_cos_to_theta(ee_ori),
Expand Down Expand Up @@ -148,6 +160,9 @@ def load_robot(self, robot_path, ee_joint_name, ee_link_name,
pybullet.getJointInfo(robot_id, j)
jname = jname.decode("utf-8")
link_name = link_name.decode("utf-8")
# take care of continuous joints
if jhighlim < jlowlim:
jlowlim, jhighlim = -np.pi, np.pi
# print('load jname', jname, 'jtype', jtype, 'link_name', link_name)
if jtype in [pybullet.JOINT_REVOLUTE, pybullet.JOINT_PRISMATIC]:
joint_ids.append(j)
Expand Down Expand Up @@ -198,7 +213,7 @@ def reset(self):
self.reset_to_qpos(self.rest_qpos)

def reset_to_qpos(self, qpos):
qpos = np.clip(qpos, self.info.joint_minpos, self.info.joint_maxpos)
qpos = self.clip_qpos(qpos)
for jid in range(self.info.dof):
self.sim.resetJointState(
bodyUniqueId=self.info.robot_id,
Expand All @@ -212,7 +227,7 @@ def clear_motor_control(self):
# torque control values this way are hard to reproduce.
self.sim.setJointMotorControlArray(
self.info.robot_id, self.info.joint_ids.tolist(),
pybullet.VELOCITY_CONTROL, forces=[0]*self.info.dof)
pybullet.VELOCITY_CONTROL, targetVelocities=[0]*self.info.dof)
self.sim.setJointMotorControlArray(
self.info.robot_id, self.info.joint_ids.tolist(),
pybullet.TORQUE_CONTROL, forces=[0]*self.info.dof)
Expand Down Expand Up @@ -281,18 +296,8 @@ def _ee_pos_to_qpos_raw(self, ee_pos, ee_ori=None, fing_dist=0.0,
qpos = pybullet.calculateInverseKinematics(
self.info.robot_id, self.info.ee_link_id,
targetPosition=ee_pos.tolist(), targetOrientation=ee_quat,
lowerLimits=self.info.joint_minpos.tolist(),
upperLimits=self.info.joint_maxpos.tolist(),
jointRanges=(self.info.joint_maxpos-self.info.joint_minpos).tolist(),
restPoses=self.rest_qpos.tolist(),
maxNumIterations=500, residualThreshold=0.005)
# solver=pybullet.IK_SDLS,
# maxNumIterations=1000, residualThreshold=0.0001)
# Note that large num iterations could slow down the compute enough
# s.t. visualizer shows differences in rate of following traj.
**self.default_ik_args)
qpos = np.array(qpos)
if debug:
print('_ee_pos_to_qpos_raw() qpos from IK', qpos)
for jid in self.info.finger_jids_lst:
qpos[jid] = np.clip( # finger info (not set by IK)
fing_dist/2.0, self.info.joint_minpos[jid],
Expand All @@ -308,12 +313,7 @@ def _ee_pos_to_qpos_raw(self, ee_pos, ee_ori=None, fing_dist=0.0,
left_qpos = np.array(pybullet.calculateInverseKinematics(
self.info.robot_id, self.info.left_ee_link_id,
left_ee_pos.tolist(), left_ee_quat,
lowerLimits=self.info.joint_minpos.tolist(),
upperLimits=self.info.joint_maxpos.tolist(),
jointRanges=(self.info.joint_maxpos -
self.info.joint_minpos).tolist(),
restPoses=self.rest_qpos.tolist(),
maxNumIterations=500, residualThreshold=0.005))
**self.default_ik_args))
else:
left_qpos = self.get_qpos()
qpos[self.info.left_arm_jids_lst] = \
Expand All @@ -323,7 +323,7 @@ def _ee_pos_to_qpos_raw(self, ee_pos, ee_ori=None, fing_dist=0.0,
left_fing_dist/2.0, self.info.joint_minpos[jid],
self.info.joint_maxpos[jid])
# IK will find solutions outside of joint limits, so clip.
# qpos = np.clip(qpos, self.info.joint_minpos, self.info.joint_maxpos)
qpos = self.clip_qpos(qpos)
return qpos

def move_to_qpos(self, tgt_qpos, mode, kp=None, kd=None):
Expand Down Expand Up @@ -373,8 +373,7 @@ def move_to_qposvel(self, tgt_qpos, tgt_qvel, mode, kp, kd):
pybullet.PD_CONTROL])
kps = kp if type(kp)==list else [kp]*self.info.dof
kds = kd if type(kd)==list else [kd]*self.info.dof
rbt_tgt_qpos = np.clip(
tgt_qpos, self.info.joint_minpos, self.info.joint_maxpos)
rbt_tgt_qpos = self.clip_qpos(tgt_qpos)
rbt_tgt_qvel = np.clip(
tgt_qvel, -1.0*self.info.joint_maxvel, self.info.joint_maxvel)
# PD example: https://github.com/bulletphysics/bullet3/issues/2152
Expand Down Expand Up @@ -412,7 +411,7 @@ def move_to_qposvel(self, tgt_qpos, tgt_qvel, mode, kp, kd):
positionGains=kps, # e.g. 100.0
velocityGains=kds, # e.g. 10.0
forces=self.info.joint_maxforce.tolist()) # see docs page 22
# self.obey_joint_limits()
self.obey_joint_limits()

def move_to_ee_pos(self, tgt_ee_pos, tgt_ee_quat=None, fing_dist=0.0,
left_ee_pos=None, left_ee_quat=None, left_fing_dist=0.0,
Expand Down Expand Up @@ -511,25 +510,34 @@ def inverse_dynamics(self, des_acc):
self.info.robot_id, qpos.tolist(), qvel.tolist(), des_acc.tolist())
return np.array(torques)

def obey_joint_limits(self):
qpos = self.get_qpos()
def clip_qpos(self, qpos):
if ((qpos>=self.info.joint_minpos).all() and
(qpos<=self.info.joint_maxpos).all()):
return qpos
clipped_qpos = np.copy(qpos)
for jid in range(self.info.dof):
jpos = qpos[jid]; jvel = 0; ok = True
jpos = qpos[jid]
if jpos < self.info.joint_minpos[jid]:
jpos = self.info.joint_minpos[jid]; ok = False
jpos = self.info.joint_minpos[jid]
if jpos > self.info.joint_maxpos[jid]:
jpos = self.info.joint_maxpos[jid]; ok = False
if not ok:
if self.debug:
print('fixing joint', self.info.joint_ids[jid],
'from pos', qpos[jid], 'to', jpos)
self.sim.resetJointState(
bodyUniqueId=self.info.robot_id,
jointIndex=self.info.joint_ids[jid],
targetValue=jpos, targetVelocity=jvel)
jpos = self.info.joint_maxpos[jid]
clipped_qpos[jid] = jpos
return clipped_qpos

def obey_joint_limits(self):
qpos = self.get_qpos()
assert((qpos>=self.info.joint_minpos).all())
assert((qpos<=self.info.joint_maxpos).all())
clipped_qpos = self.clip_qpos(qpos)
neq_ids = np.nonzero(qpos - clipped_qpos)[0]
for jid in neq_ids:
if self.debug:
print('fix jid', jid, qpos[jid], '->', clipped_qpos[jid])
self.sim.resetJointState(
bodyUniqueId=self.info.robot_id,
jointIndex=self.info.joint_ids[jid],
targetValue=clipped_qpos[jid], targetVelocity=0)
# qpos = self.get_qpos()
# assert((qpos>=self.info.joint_minpos).all())
# assert((qpos<=self.info.joint_maxpos).all())

def ee_pos_to_qpos(self, ee_pos, ee_ori, fing_dist, left_ee_pos=None,
left_ee_ori=None, left_fing_dist=0.0, debug=False):
Expand Down
14 changes: 7 additions & 7 deletions dedo/utils/preset_info.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -133,18 +133,18 @@
'waypoints': {
'a': [
# [0.4332, 1.9885, 6.1941],
[1, 0, 7.1, 1],
[1.0, -0.5, 6.9, 0.5],
[1, 0, 7.2, 1],
[1.0, -0.5, 7.1, 0.5],
[0.3, -1, 7.0, 0.5],
[0.3, -1, 7.1, 0.5],
[0.3, -1, 6.7, 0.5],
# [0.3, -1, 7.1, 0.5],
# [0.3, -1, 6.7, 0.5],
],
'b': [
# [-0.8332 , 1.9885 , 6.1941]
[-1.5, 0, 7.1, 1],
[-1.7, -0.5, 6.9, 0.5],
[-1.5, 0, 7.2, 1],
[-1.7, -0.5, 7.1, 0.5],
[-0.6, -1.2, 7.0, 0.5],
[-0.6, -1.2, 6.0, 0.5],
# [-0.6, -1.2, 6.0, 0.5],

]
},
Expand Down

0 comments on commit 8a0b4dd

Please sign in to comment.