Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Custom gripper interface and tutorial section #5

Merged
merged 4 commits into from
Nov 25, 2023
Merged

Custom gripper interface and tutorial section #5

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
7bc3630
Move to radian/quat description
JeanElsner Nov 25, 2023
b8a522f
Custom gripper interface and tutorial section
JeanElsner Nov 25, 2023
6bd75d5
Add custom gripper test
JeanElsner Nov 25, 2023
1b469cd
Additional tests
JeanElsner Nov 25, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Binary file added doc/img/custom_gripper.png
View file
Open in desktop
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
29 changes: 28 additions & 1 deletion doc/tutorial.rst
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -311,6 +311,33 @@ body to rotate it, while the motion remains invariant.

.. youtube:: cAUjkhrBmN4

Custom Gripper
--------------

You can use the Panda model with any gripper that implements the ``dm_robotics`` gripper interface.
The example implemented in ``examples/custom_gripper.py`` uses a model of the `Robotiq 2-finger 85`
gripper, sensor and effector to attach to a Panda robot. This is again done using the robot configuration.

.. code:: python

gripper = robotiq_2f85.Robotiq2F85()
gripper_params = params.GripperParams(
model=gripper,
effector=default_gripper_effector.DefaultGripperEffector(
gripper, 'robotique'),
sensors=[
robotiq_gripper_sensor.RobotiqGripperSensor(gripper, 'robotique')
])
robot_params = params.RobotParams(gripper=gripper_params, has_hand=False)

Gripper parameters above include model, sensors and an effector that are part of the robot configuration.
You also need to set ``has_hand`` to `False`, otherwise the Panda gripper will be used. The Robotiq gripper
can be controlled continuously and the example uses a simple agent to apply a sinusoidal action to control
the gripper. The resulting model is pictured below.

.. image:: img/custom_gripper.png
:alt: Custom Gripper

RL Environment
--------------

Expand Down Expand Up @@ -374,7 +401,7 @@ The latter allows us to attach cameras to the robots as well.
panda_env.robots[robot_params.name].gripper.tool_center_point.parent.add(
'camera',
pos=(.1, 0, -.1),
euler=(180, 0, -90),
euler=(np.pi, 0, -np.pi / 2),
fovy=90,
name='wrist_camera')

Expand Down
12 changes: 3 additions & 9 deletions examples/assets/two_arm.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,23 +4,17 @@
<texture name='groundplane' type='2d' builtin='checker' rgb1='.2 .3 .4' rgb2='.1 .2 .3' width='300' height='300' mark='edge' markrgb='.8 .8 .8'/>
<material name='groundplane' texture='groundplane' texrepeat='5 5' texuniform='true' reflectance='.2'/>
</asset>

<!-- Always initialize the free camera to point at the origin. -->
<statistic center='0 0 0'/>
<default>
<geom solimp="0.9 0.95 0.001 0.5 2" solref="0.005 1" condim="6" />
</default>
<worldbody>
<geom name='ground' type='plane' size='2 2 2' material='groundplane'
friction='0.4'/>
<light directional='true' diffuse='.7 .7 .7' pos='1 .1 2' dir='0 -.1 -2' specular='.3 .3 .3' castshadow='true'/>
<body pos="0 0 1" euler="0 0 30">
<body pos="0 0 1" quat="0.9659 0 0 0.2588">
<joint type="hinge" axis="0 0 1" damping="2000" />
<geom type="box" size=".15 .2 .15" />
<geom type="cylinder" size=".1 .3" />
<geom type="sphere" size=".2" pos="0 0 .4" />
<site name="left" pos="0 .2 0" euler="-90 0 0" />
<site name="right" pos="0 -.2 0" euler="90 0 0" />
<site name="left" pos="0 .2 0" quat="1 -1 0 0" />
<site name="right" pos="0 -.2 0" quat="1 1 0 0" />
<site name="control" />
</body>
</worldbody>
Expand Down
60 changes: 60 additions & 0 deletions examples/custom_gripper.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,60 @@
"""Example showing how to use a custom gripper with the Panda model."""
import dm_env
import numpy as np
from dm_env import specs
from dm_robotics.moma.effectors import default_gripper_effector
from dm_robotics.moma.models.end_effectors.robot_hands import robotiq_2f85
from dm_robotics.moma.sensors import robotiq_gripper_sensor

from dm_robotics.panda import environment
from dm_robotics.panda import parameters as params
from dm_robotics.panda import run_loop, utils


class Agent:
"""Agents are used to control a robot's actions given
current observations and rewards.
"""

def __init__(self, spec: specs.BoundedArray) -> None:
self._spec = spec

def step(self, timestep: dm_env.TimeStep) -> np.ndarray:
"""Sinusoidal movement of the gripper's fingers."""
time = timestep.observation['time'][0]
action = np.zeros(shape=self._spec.shape, dtype=self._spec.dtype)
action[6] = 255 * np.sin(time)
return action


if __name__ == '__main__':
# We initialize the default configuration for logging
# and argument parsing. These steps are optional.
utils.init_logging()
parser = utils.default_arg_parser()
args = parser.parse_args()

gripper = robotiq_2f85.Robotiq2F85()
gripper_params = params.GripperParams(
model=gripper,
effector=default_gripper_effector.DefaultGripperEffector(
gripper, 'robotique'),
sensors=[
robotiq_gripper_sensor.RobotiqGripperSensor(gripper, 'robotique')
])

# Use RobotParams to customize Panda robots added to the environment.
robot_params = params.RobotParams(gripper=gripper_params, has_hand=False)
panda_env = environment.PandaEnvironment(robot_params)

with panda_env.build_task_environment() as env:
# Print the full action, observation and reward specification
utils.full_spec(env)
# Initialize the agent
agent = Agent(env.action_spec())
# Run the environment and agent either in headless mode or inside the GUI.
if args.gui:
app = utils.ApplicationWithPlot()
app.launch(env, policy=agent.step)
else:
run_loop.run(env, agent, [], max_steps=1000, real_time=True)
2 changes: 1 addition & 1 deletion examples/rl_environment.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -74,7 +74,7 @@ def goal_reward(observation: spec_utils.ObservationValue):
panda_env.robots[robot_params.name].gripper.tool_center_point.parent.add(
'camera',
pos=(.1, 0, -.1),
euler=(180, 0, -90),
euler=(np.pi, 0, -np.pi / 2),
fovy=90,
name='wrist_camera')

Expand Down
21 changes: 13 additions & 8 deletions src/dm_robotics/panda/arm.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -491,16 +491,21 @@

ns_gripper = f'{robot_params.name}_gripper'
if robot_params.has_hand:
_gripper = gripper_module.PandaHand(name=ns_gripper)
panda_hand_sensor = gripper_module.PandaHandSensor(_gripper, ns_gripper)
gripper = gripper_module.PandaHand(name=ns_gripper)
panda_hand_sensor = gripper_module.PandaHandSensor(gripper, ns_gripper)
robot_sensors.append(panda_hand_sensor)
gripper_effector = gripper_module.PandaHandEffector(robot_params, _gripper,
gripper_effector = gripper_module.PandaHandEffector(robot_params, gripper,
panda_hand_sensor)
elif robot_params.gripper is not None:
gripper = robot_params.gripper.model
if robot_params.gripper.sensors is not None:
robot_sensors.extend(robot_params.gripper.sensors)
gripper_effector = robot_params.gripper.effector
else:
_gripper = gripper_module.DummyHand(name=ns_gripper)
gripper = gripper_module.DummyHand(name=ns_gripper)

Check warning on line 505 in src/dm_robotics/panda/arm.py

View check run for this annotation

Codecov / codecov/patch

src/dm_robotics/panda/arm.py#L505 

Added line #L505 was not covered by tests
gripper_effector = None

tcp_sensor = RobotTCPSensor(_gripper, robot_params)
tcp_sensor = RobotTCPSensor(gripper, robot_params)
robot_sensors.extend([
ExternalWrenchObserver(robot_params, arm, arm_sensor), tcp_sensor,
arm_sensor
Expand All @@ -513,14 +518,14 @@
_arm_effector = ArmEffector(robot_params, arm)
elif robot_params.actuation == consts.Actuation.CARTESIAN_VELOCITY:
joint_velocity_effector = ArmEffector(robot_params, arm)
_arm_effector = Cartesian6dVelocityEffector(robot_params, arm, _gripper,
_arm_effector = Cartesian6dVelocityEffector(robot_params, arm, gripper,
joint_velocity_effector,
tcp_sensor)

robot.standard_compose(arm, _gripper)
robot.standard_compose(arm, gripper)
moma_robot = robot.StandardRobot(arm=arm,
arm_base_site_name=arm.base_site.name,
gripper=_gripper,
gripper=gripper,
robot_sensors=robot_sensors,
arm_effector=_arm_effector,
gripper_effector=gripper_effector)
Expand Down
28 changes: 14 additions & 14 deletions src/dm_robotics/panda/assets/panda/panda.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
<mujoco model="panda">
<compiler angle="degree" />
<compiler angle="radian" />
<asset>
<mesh name="link0" file="link0.obj" />
<mesh name="link1" file="link1.obj" />
Expand Down Expand Up @@ -32,37 +32,37 @@
<body name="panda_link1" pos="0 0 0.333" gravcomp="1">
<site name="panda_joint_site1" />
<inertial pos="3.875e-03 2.081e-03 -4.762e-02" mass="4.970684" fullinertia="7.0337e-01 7.0661e-01 9.1170e-03 -1.3900e-04 6.7720e-03 1.9169e-02" />
<joint name="panda_joint1" pos="0 0 0" axis="0 0 1" limited="true" range="-166 166" damping="0.0665" frictionloss="0.2450" />
<joint name="panda_joint1" pos="0 0 0" axis="0 0 1" limited="true" range="-2.897 2.897" damping="0.0665" frictionloss="0.2450" />
<geom type="mesh" material="panda_white" mesh="link1" />
<body name="panda_link2" pos="0 0 0" euler="-90 0 -45" gravcomp="1">
<body name="panda_link2" pos="0 0 0" quat="1 -1 0 0" gravcomp="1">
<site name="panda_joint_site2" />
<inertial pos="-3.141e-03 -2.872e-02 3.495e-03" mass="0.646926" fullinertia="7.9620e-03 2.8110e-02 2.5995e-02 -3.9250e-03 1.0254e-02 7.0400e-04" />
<joint name="panda_joint2" ref="-45" pos="0 0 0" axis="0 0 1" limited="true" range="-101 101" damping="0.1987" frictionloss="0.1523" />
<joint name="panda_joint2" pos="0 0 0" axis="0 0 1" limited="true" range="-1.7627 1.7627" damping="0.1987" frictionloss="0.1523" />
<geom type="mesh" material="panda_white" mesh="link2" />
<body name="panda_link3" pos="0 -0.316 0" euler="90 0 0" gravcomp="1">
<body name="panda_link3" pos="0 -0.316 0" quat="1 1 0 0" gravcomp="1">
<site name="panda_joint_site3" />
<inertial pos="2.7518e-02 3.9252e-02 -6.6502e-02" mass="3.228604" fullinertia="3.7242e-02 3.6155e-02 1.0830e-02 -4.7610e-03 -1.1396e-02 -1.2805e-02" />
<joint name="panda_joint3" pos="0 0 0" axis="0 0 1" limited="true" range="-166 166" damping="0.0399" frictionloss="0.1827" />
<joint name="panda_joint3" pos="0 0 0" axis="0 0 1" limited="true" range="-2.897 2.897" damping="0.0399" frictionloss="0.1827" />
<geom type="mesh" material="panda" mesh="link3" />
<body name="panda_link4" pos="0.0825 0 0" euler="90 0 -135" gravcomp="1">
<body name="panda_link4" pos="0.0825 0 0" quat="1 1 0 0" gravcomp="1">
<site name="panda_joint_site4" />
<inertial pos="-5.317e-02 1.04419e-01 2.7454e-02" mass="3.587895" fullinertia="2.5853e-02 1.9552e-02 2.8323e-02 7.7960e-03 -1.3320e-03 8.6410e-03" />
<joint name="panda_joint4" ref="-135" pos="0 0 0" axis="0 0 1" limited="true" range="-176 -4" damping="0.2257" frictionloss="0.3591" />
<joint name="panda_joint4" pos="0 0 0" axis="0 0 1" limited="true" range="-3.0717 -0.0698" damping="0.2257" frictionloss="0.3591" />
<geom type="mesh" material="panda" mesh="link4" />
<body name="panda_link5" pos="-0.0825 0.384 0" euler="-90 0 0" gravcomp="1">
<body name="panda_link5" pos="-0.0825 0.384 0" quat="1 -1 0 0" gravcomp="1">
<site name="panda_joint_site5" />
<inertial pos="-1.1953e-02 4.1065e-02 -3.8437e-02" mass="1.225946" fullinertia="3.5549e-02 2.9474e-02 8.6270e-03 -2.1170e-03 -4.0370e-03 2.2900e-04" />
<joint name="panda_joint5" pos="0 0 0" axis="0 0 1" limited="true" range="-166 166" damping="0.1023" frictionloss="0.2669" />
<joint name="panda_joint5" pos="0 0 0" axis="0 0 1" limited="true" range="-2.897 2.897" damping="0.1023" frictionloss="0.2669" />
<geom type="mesh" material="panda" mesh="link5" />
<body name="panda_link6" pos="0 0 0" euler="90 0 90" gravcomp="1">
<body name="panda_link6" pos="0 0 0" quat="1 1 0 0" gravcomp="1">
<site name="panda_joint_site6" />
<inertial pos="6.0149e-02 -1.4117e-02 -1.0517e-02" mass="1.666555 " fullinertia="1.9640e-03 4.3540e-03 5.4330e-03 1.0900e-04 -1.1580e-03 3.4100e-04" />
<joint name="panda_joint6" ref="90" pos="0 0 0" axis="0 0 1" limited="true" range="-1 215" damping="-0.0132" frictionloss="0.1658" />
<joint name="panda_joint6" pos="0 0 0" axis="0 0 1" limited="true" range="-0.0174 3.7524" damping="-0.0132" frictionloss="0.1658" />
<geom type="mesh" material="panda" mesh="link6" />
<body name="panda_link7" pos="0.088 0 0" euler="90 0 45" gravcomp="1">
<body name="panda_link7" pos="0.088 0 0" quat="1 1 0 0" gravcomp="1">
<site name="panda_joint_site7" />
<inertial pos="1.0517e-02 -4.252e-03 6.1597e-02" mass="7.35522e-01" fullinertia="1.2516e-02 1.0027e-02 4.8150e-03 -4.2800e-04 -1.1960e-03 -7.4100e-04" />
<joint name="panda_joint7" ref="45" pos="0 0 0" axis="0 0 1" limited="true" range="-166 166" damping="0.0638" frictionloss="1.2109" />
<joint name="panda_joint7" pos="0 0 0" axis="0 0 1" limited="true" range="-2.897 2.897" damping="0.0638" frictionloss="1.2109" />
<geom type="mesh" material="panda" mesh="link7" />
<body name="panda_link8" pos="0 0 0.107">
<site name="wrist_site" />
Expand Down
11 changes: 5 additions & 6 deletions src/dm_robotics/panda/assets/panda/panda_hand.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
<mujoco model="panda_hand">
<compiler angle="degree" />
<compiler angle="radian" />
<asset>
<mesh name="link0" file="link0.obj" />
<mesh name="link1" file="link1.obj" />
Expand All @@ -19,17 +19,17 @@
<joint name="couple_fingers" joint1="panda_finger_joint1" joint2="panda_finger_joint2" solimp="0.95 0.99 0.001" solref="0.005 1"/>
</equality>
<worldbody>
<body name="panda_hand" euler="0 0 -0.785398163397" gravcomp="1">
<body name="panda_hand" quat="1 0 0 0" gravcomp="1">
<site name="TCP" pos="0 0 .1034" />
<inertial pos="-1e-02 0 3e-02" mass="7.3e-01" diaginertia="1e-03 2.5e-03 1.7e-03" />
<geom type="mesh" material="panda" mesh="hand" />
<body name="panda_leftfinger" pos="0 0 0.0584" euler="0 0 180" gravcomp="1">
<body name="panda_leftfinger" pos="0 0 0.0584" quat="0 0 0 1" gravcomp="1">
<inertial pos="0 0 0" mass="1.5e-02" diaginertia="2.3749e-06 2.3749e-06 7.5e-07"/>
<joint name="panda_finger_joint1" pos="0 0 0" axis="0 1 0" type="slide" limited="true" range="0 0.04" armature="0.1" damping="11" solreflimit="-30000 -200" />
<geom type="mesh" material="panda" mesh="finger" contype="0" conaffinity="0" />
<geom type="box" name="panda_leftfinger_collision" pos="0 0.015 0.032" size="0.012 0.015 0.022" rgba="1 1 1 .3" group="5"/>
</body>
<body name="panda_rightfinger" pos="0 0 0.0584" euler="0 0 0" gravcomp="1">
<body name="panda_rightfinger" pos="0 0 0.0584" quat="1 0 0 0" gravcomp="1">
<inertial pos="0 0 0" mass="1.5e-02" diaginertia="2.3749e-06 2.3749e-06 7.5e-07"/>
<joint name="panda_finger_joint2" pos="0 0 0" axis="0 1 0" type="slide" limited="true" range="0 0.04" armature="0.1" damping="11" solreflimit="-30000 -200" />
<geom type="mesh" material="panda" mesh="finger" contype="0" conaffinity="0" />
Expand All @@ -44,7 +44,6 @@
</fixed>
</tendon>
<actuator>
<!-- Remap original ctrlrange (0, 0.04) to (0, 1): 0.04 * 100 / 1 = 4 -->
<general biastype="affine" name="panda_hand_actuator" tendon="split" forcelimited="true" forcerange="-100 100" ctrllimited="true" ctrlrange="0 1" gainprm="4 0 0" biasprm="0 -100 0"/>
<general biastype="affine" name="panda_hand_actuator" tendon="split" forcelimited="true" forcerange="-100 100" ctrllimited="true" ctrlrange="0 1" gainprm="240 0 0" biasprm="0 -6000 -10"/>
</actuator>
</mujoco>
2 changes: 0 additions & 2 deletions src/dm_robotics/panda/environment.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,8 +32,6 @@ def __init__(self,
self._robot_params = robot_params if isinstance(
robot_params, Sequence) else [robot_params]
self._control_timestep = control_timestep

self._arena.mjcf_model.compiler.angle = 'degree'
self._arena.mjcf_model.option.timestep = physics_timestep

self._robots = collections.OrderedDict()
Expand Down
5 changes: 5 additions & 0 deletions src/dm_robotics/panda/hardware.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -234,6 +234,11 @@ def build_robot(robot_params: params.RobotParams) -> robot.Robot:
robot_sensors.append(panda_hand_sensor)
gripper_effector = PandaHandEffector(robot_params, gripper,
panda_hand_sensor, hardware_gripper)
elif robot_params.gripper is not None:
gripper = robot_params.gripper.model
if robot_params.gripper.sensors is not None:
robot_sensors.extend(robot_params.gripper.sensors)
gripper_effector = robot_params.gripper.effector
else:
gripper = gripper_module.DummyHand(name=ns_gripper)
gripper_effector = None
Expand Down
11 changes: 11 additions & 0 deletions src/dm_robotics/panda/parameters.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,10 +3,20 @@
from typing import Optional, Sequence

from dm_control import mjcf
from dm_robotics.moma import effector, sensor
from dm_robotics.moma.models.end_effectors.robot_hands import robot_hand

from . import arm_constants


@dataclasses.dataclass
class GripperParams:
"""Parameters describing a custom gripper."""
model: robot_hand.RobotHand
effector: effector.Effector
sensors: Optional[Sequence[sensor.Sensor]] = None


@dataclasses.dataclass
class RobotParams:
"""Parameters used for building the Panda robot model.
Expand All @@ -33,6 +43,7 @@ class RobotParams:
control_frame: Optional[mjcf.Element] = None
actuation: arm_constants.Actuation = arm_constants.Actuation.CARTESIAN_VELOCITY
has_hand: bool = True
gripper: Optional[GripperParams] = None
robot_ip: Optional[str] = None
joint_stiffness: Sequence[float] = (600, 600, 600, 600, 250, 150, 50)
joint_damping: Sequence[float] = (50, 50, 50, 20, 20, 20, 10)
17 changes: 17 additions & 0 deletions test/test_arm.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,8 @@
import numpy as np
from dm_control import mjcf
from dm_robotics.moma.effectors import default_gripper_effector
from dm_robotics.moma.models.end_effectors.robot_hands import robotiq_2f85
from dm_robotics.moma.sensors import robotiq_gripper_sensor
from numpy import testing

from dm_robotics.panda import arm, arm_constants, parameters
Expand Down Expand Up @@ -27,6 +30,7 @@ def test_arm_effector():
physics = mjcf.Physics.from_mjcf_model(robot.mjcf_model)
effector = arm.ArmEffector(robot_params, robot)
effector.set_control(physics, np.zeros(7, dtype=np.float32))
robot.after_substep(physics, np.random.RandomState(0))
effector.close()


Expand All @@ -37,3 +41,16 @@ def test_arm_haptic():
physics = mjcf.Physics.from_mjcf_model(robot.mjcf_model)
effector = arm.ArmEffector(robot_params, robot)
effector.set_control(physics, np.zeros(0, dtype=np.float32))


def test_custom_gripper():
gripper = robotiq_2f85.Robotiq2F85()
gripper_params = parameters.GripperParams(
model=gripper,
effector=default_gripper_effector.DefaultGripperEffector(
gripper, 'robotique'),
sensors=[
robotiq_gripper_sensor.RobotiqGripperSensor(gripper, 'robotique')
])
arm.build_robot(parameters.RobotParams(gripper=gripper_params,
has_hand=False))
6 changes: 4 additions & 2 deletions test/test_environment.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
import numpy as np
from dm_control import mjcf
from dm_control.composer.variation import distributions, rotations
from dm_env import specs
from dm_robotics.agentflow.preprocessors import rewards
from dm_robotics.moma import effector, entity_initializer, prop
Expand Down Expand Up @@ -58,13 +59,14 @@ def test_components():
panda_env.add_props(props)

entity_initializers = [
entity_initializer.PropPlacer(props, [0, 0, 0], [1, 0, 0, 0])
entity_initializer.prop_initializer.PropPlacer(
props, distributions.Uniform(-1, 1), rotations.UniformQuaternion())
]
panda_env.add_extra_effectors(extra_effectors)
panda_env.add_extra_sensors(extra_sensors)
panda_env.add_entity_initializers(entity_initializers)
panda_env.add_scene_initializers([initialize_scene])
panda_env.add_timestep_preprocessors(preprocessors)

with panda_env.build_task_environment():
with panda_env.build_task_environment() as env:
pass
Loading
Loading