mirror of
https://github.com/wassname/pyrobolearn.git
synced 2026-09-09 11:31:38 +08:00
update mujoco + ROS + real experiments with Franka
This commit is contained in:
Brian Delhaisse
@@ -0,0 +1,340 @@
|
||||
<?xml version="1.0" ?>
|
||||
<!-- =================================================================================== -->
|
||||
<!-- | This document was autogenerated by xacro from panda_arm_hand.urdf.xacro | -->
|
||||
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||
<!-- =================================================================================== -->
|
||||
<robot name="panda" xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<material name="black">
|
||||
<color rgba="0.0 0.0 0.0 1"/>
|
||||
</material>
|
||||
<material name="white">
|
||||
<color rgba="0.9 0.9 0.9 1"/>
|
||||
</material>
|
||||
<material name="dark_gray">
|
||||
<color rgba="0.3 0.3 0.3 1"/>
|
||||
</material>
|
||||
<material name="gray">
|
||||
<color rgba="0.5 0.5 0.5 1"/>
|
||||
</material>
|
||||
<material name="light_gray">
|
||||
<color rgba="0.7 0.7 0.7 1"/>
|
||||
</material>
|
||||
<material name="pure_red">
|
||||
<color rgba="1.0 0.0 0.0 1"/>
|
||||
</material>
|
||||
<material name="red">
|
||||
<color rgba="0.5 0.1 0.2 1"/>
|
||||
</material>
|
||||
<material name="pure_green">
|
||||
<color rgba="0.0 1.0 0.0 1"/>
|
||||
</material>
|
||||
<material name="pure_blue">
|
||||
<color rgba="0.0 0.0 1.0 1"/>
|
||||
</material>
|
||||
<material name="blue">
|
||||
<color rgba="0.0 0.2 0.3 1"/>
|
||||
</material>
|
||||
|
||||
<link name="panda_link0">
|
||||
<inertial> <!-- these inertia are calculated based on the visual meshes using Meshlab, and using a density of 1kg/m^3 (http://gazebosim.org/tutorials?tut=inertia) -->
|
||||
<mass value="1"/> <!-- volume = 0.002442 -->
|
||||
<origin xyz="-0.025566 -0.000029 0.057332"/>
|
||||
<!--inertia ixx="0.000007" ixy="0.0" ixz="-0.000001" iyy="0.000009" iyz="0.0" izz="0.000009"/-->
|
||||
<inertia ixx="0.00286650" ixy="0.0" ixz="-0.0004095" iyy="0.00368550" iyz="0.0" izz="0.00368550"/> <!-- divided by the volume, just need to be multiplied by the mass -->
|
||||
</inertial>
|
||||
<visual>
|
||||
<material name="white"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link0.stl"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link0.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="panda_link1">
|
||||
<inertial> <!-- these inertia are calculated based on the visual meshes using Meshlab, and using a density of 1kg/m^3 (http://gazebosim.org/tutorials?tut=inertia) -->
|
||||
<mass value="1"/> <!-- volume = 0.002293 -->
|
||||
<origin xyz="-0.000000 -0.032496 -0.067582"/>
|
||||
<!--inertia ixx="0.000015" ixy="0.0" ixz="0.0" iyy="0.000013" iyz="0.000004" izz="0.000005"/-->
|
||||
<inertia ixx="0.00654165" ixy="0.0" ixz="0.0" iyy="0.00566943" iyz="0.00174444" izz="0.00218055"/> <!-- divided by the volume, just need to be multiplied by the mass -->
|
||||
</inertial>
|
||||
<visual>
|
||||
<material name="white"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link1.stl"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link1.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint1" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.333"/>
|
||||
<parent link="panda_link0"/>
|
||||
<child link="panda_link1"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="87" lower="-2.9671" upper="2.9671" velocity="2.1750"/>
|
||||
</joint>
|
||||
<link name="panda_link2">
|
||||
<inertial> <!-- these inertia are calculated based on the visual meshes using Meshlab, and using a density of 1kg/m^3 (http://gazebosim.org/tutorials?tut=inertia) -->
|
||||
<mass value="1"/> <!-- volume = 0.002312 -->
|
||||
<origin xyz="0.000000 -0.068610 0.032229"/>
|
||||
<!--inertia ixx="0.000015" ixy="0.0" ixz="0.0" iyy="0.000005" iyz="0.000004" izz="0.000014"/-->
|
||||
<inertia ixx="0.00648789" ixy="0.0" ixz="0.0" iyy="0.00216263" iyz="0.0017301" izz="0.00605536"/> <!-- divided by the volume, just need to be multiplied by the mass -->
|
||||
</inertial>
|
||||
<visual>
|
||||
<material name="white"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link2.stl"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link2.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint2" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
|
||||
<origin rpy="-1.57079632679 0 0" xyz="0 0 0"/>
|
||||
<parent link="panda_link1"/>
|
||||
<child link="panda_link2"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="87" lower="-1.8326" upper="1.8326" velocity="2.1750"/>
|
||||
</joint>
|
||||
<link name="panda_link3">
|
||||
<inertial> <!-- these inertia are calculated based on the visual meshes using Meshlab, and using a density of 1kg/m^3 (http://gazebosim.org/tutorials?tut=inertia) -->
|
||||
<mass value="1"/> <!-- volume = 0.002021 -->
|
||||
<origin xyz="0.046989 0.031637 -0.031704"/>
|
||||
<!--inertia ixx="0.000007" ixy="-0.000002" ixz="-0.000003" iyy="0.000008" iyz="-0.000002" izz="0.000007"/-->
|
||||
<inertia ixx="0.00346363" ixy="-0.0009896" ixz="-0.00148441" iyy="0.00395844" iyz="-0.0009896" izz="0.00346363"/> <!-- divided by the volume, just need to be multiplied by the mass -->
|
||||
</inertial>
|
||||
<visual>
|
||||
<!-- <material name="light_gray"/> -->
|
||||
<material name="white"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link3.stl"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link3.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint3" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
|
||||
<origin rpy="1.57079632679 0 0" xyz="0 -0.316 0"/>
|
||||
<parent link="panda_link2"/>
|
||||
<child link="panda_link3"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="87" lower="-2.9671" upper="2.9671" velocity="2.1750"/>
|
||||
</joint>
|
||||
<link name="panda_link4">
|
||||
<inertial> <!-- these inertia are calculated based on the visual meshes using Meshlab, and using a density of 1kg/m^3 (http://gazebosim.org/tutorials?tut=inertia) -->
|
||||
<mass value="1"/> <!-- volume = 0.002004 -->
|
||||
<origin xyz="-0.036051 0.033707 0.031902"/>
|
||||
<!--inertia ixx="0.000007" ixy="0.000003" ixz="-0.000002" iyy="0.000007" iyz="0.000002" izz="0.000009"/-->
|
||||
<inertia ixx="0.00349301" ixy="0.00149701" ixz="-0.00099800" iyy="0.00349301" iyz="0.00099800" izz="0.00449102"/> <!-- divided by the volume, just need to be multiplied by the mass -->
|
||||
</inertial>
|
||||
<visual>
|
||||
<material name="white"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link4.stl"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link4.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint4" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
|
||||
<origin rpy="1.57079632679 0 0" xyz="0.0825 0 0"/>
|
||||
<parent link="panda_link3"/>
|
||||
<child link="panda_link4"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="87" lower="-3.1416" upper="0.0" velocity="2.1750"/>
|
||||
</joint>
|
||||
<link name="panda_link5">
|
||||
<inertial> <!-- these inertia are calculated based on the visual meshes using Meshlab, and using a density of 1kg/m^3 (http://gazebosim.org/tutorials?tut=inertia) -->
|
||||
<mass value="1"/> <!-- volume = 0.002275 -->
|
||||
<origin xyz="-0.000006 0.061043 -0.104176"/>
|
||||
<!--inertia ixx="0.000025" ixy="0.000000" ixz="-0.000000" iyy="0.000024" iyz="-0.000006" izz="0.000004"/-->
|
||||
<inertia ixx="0.01098901" ixy="0.000000" ixz="0.000000" iyy="0.01054945" iyz="-0.00263736" izz="0.00175824"/> <!-- divided by the volume, just need to be multiplied by the mass -->
|
||||
</inertial>
|
||||
<visual>
|
||||
<material name="white"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link5.stl"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link5.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint5" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
|
||||
<origin rpy="-1.57079632679 0 0" xyz="-0.0825 0.384 0"/>
|
||||
<parent link="panda_link4"/>
|
||||
<child link="panda_link5"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="12" lower="-2.9671" upper="2.9671" velocity="2.6100"/>
|
||||
</joint>
|
||||
<link name="panda_link6">
|
||||
<inertial> <!-- these inertia are calculated based on the visual meshes using Meshlab, and using a density of 1kg/m^3 (http://gazebosim.org/tutorials?tut=inertia) -->
|
||||
<mass value="1"/> <!-- volume = 0.001435 --> <!-- NOTE: BECAUSE MESHLAB OUTPUTED NAN FOR THE VISUAL MESH, I USED THE COLLISION MESH -->
|
||||
<origin xyz="0.051002 0.006933 0.006169"/>
|
||||
<!--inertia ixx="0.000002" ixy="0.000000" ixz="0.000000" iyy="0.000004" iyz="0.000000" izz="0.000004"/-->
|
||||
<inertia ixx="0.00139373" ixy="0.000000" ixz="0.000000" iyy="0.00278745" iyz="0.000000" izz="0.00278745"/> <!-- divided by the volume, just need to be multiplied by the mass -->
|
||||
</inertial>
|
||||
<visual>
|
||||
<material name="white"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link6.stl"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link6.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint6" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
|
||||
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
|
||||
<parent link="panda_link5"/>
|
||||
<child link="panda_link6"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="12" lower="-0.0873" upper="3.8223" velocity="2.6100"/>
|
||||
</joint>
|
||||
<link name="panda_link7">
|
||||
<inertial> <!-- these inertia are calculated based on the visual meshes using Meshlab, and using a density of 1kg/m^3 (http://gazebosim.org/tutorials?tut=inertia) -->
|
||||
<mass value="1"/> <!-- volume = 0.000454 -->
|
||||
<origin xyz="0.010969 0.010797 0.065041"/>
|
||||
<!--inertia ixx="0.000001" ixy="0.000000" ixz="0.000000" iyy="0.000001" iyz="0.000000" izz="0.000001"/-->
|
||||
<inertia ixx="0.00220264" ixy="0.000000" ixz="0.000000" iyy="0.00220264" iyz="0.000000" izz="0.00220264"/> <!-- divided by the volume, just need to be multiplied by the mass -->
|
||||
</inertial>
|
||||
<visual>
|
||||
<material name="white"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link7.stl"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link7.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint7" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
|
||||
<origin rpy="1.57079632679 0 0" xyz="0.088 0 0"/>
|
||||
<parent link="panda_link6"/>
|
||||
<child link="panda_link7"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="12" lower="-2.9671" upper="2.9671" velocity="2.6100"/>
|
||||
</joint>
|
||||
<link name="panda_link8">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value="0"/>
|
||||
<inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
|
||||
</inertial>
|
||||
</link> <!-- in pybullet, the inertia need to be specified even for dummy links, otherwise it will put an identity inertia and a mass of 1 by default -->
|
||||
<joint name="panda_joint8" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0 0.107"/>
|
||||
<parent link="panda_link7"/>
|
||||
<child link="panda_link8"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
<joint name="panda_hand_joint" type="fixed">
|
||||
<parent link="panda_link8"/>
|
||||
<child link="panda_hand"/>
|
||||
<origin rpy="0 0 -0.785398163397" xyz="0 0 0"/>
|
||||
</joint>
|
||||
<link name="panda_hand">
|
||||
<inertial> <!-- these inertia are calculated based on the visual meshes using Meshlab, and using a density of 1kg/m^3 (http://gazebosim.org/tutorials?tut=inertia) -->
|
||||
<mass value="1"/> <!-- volume = 0.000488 -->
|
||||
<origin xyz="-0.000007 0.001524 0.027591"/>
|
||||
<!--inertia ixx="0.000002" ixy="0.000000" ixz="0.000000" iyy="0.0000003" iyz="0.000000" izz="0.000002"/-->
|
||||
<inertia ixx="0.00409836" ixy="0.000000" ixz="0.000000" iyy="0.00061475" iyz="0.000000" izz="0.00409836"/> <!-- divided by the volume, just need to be multiplied by the mass -->
|
||||
</inertial>
|
||||
<visual>
|
||||
<material name="white"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/hand.stl"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/hand.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="panda_leftfinger">
|
||||
<inertial> <!-- these inertia are calculated based on the visual meshes using Meshlab, and using a density of 1kg/m^3 (http://gazebosim.org/tutorials?tut=inertia) -->
|
||||
<mass value="1"/> <!-- volume = 0.000011 -->
|
||||
<origin xyz="0.000002 0.014564 0.022794"/>
|
||||
<!--inertia ixx="0.00000000338" ixy="0.000000" ixz="0.000000" iyy="0.000000000332" iyz="0.00000000060" izz="0.00000000078"/-->
|
||||
<!--inertia ixx="0.00030727" ixy="0.000000" ixz="0.000000" iyy="0.00003018" iyz="0.00005455" izz="0.00007091"/--> <!-- divided by the volume, just need to be multiplied by the mass -->
|
||||
</inertial>
|
||||
<visual>
|
||||
<material name="light_gray"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/finger.stl"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/finger.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="panda_rightfinger">
|
||||
<inertial> <!-- these inertia are calculated based on the visual meshes using Meshlab, and using a density of 1kg/m^3 (http://gazebosim.org/tutorials?tut=inertia) -->
|
||||
<mass value="1"/> <!-- volume = 0.000011 -->
|
||||
<origin xyz="0.000002 0.014564 0.022794"/>
|
||||
<!--inertia ixx="0.00000000338" ixy="0.000000" ixz="0.000000" iyy="0.000000000332" iyz="0.00000000060" izz="0.00000000078"/-->
|
||||
<!--inertia ixx="0.00030727" ixy="0.000000" ixz="0.000000" iyy="0.00003018" iyz="0.00005455" izz="0.00007091"/--> <!-- divided by the volume, just need to be multiplied by the mass -->
|
||||
</inertial>
|
||||
<visual>
|
||||
<material name="light_gray"/>
|
||||
<origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/finger.stl"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/finger.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_finger_joint1" type="prismatic">
|
||||
<parent link="panda_hand"/>
|
||||
<child link="panda_leftfinger"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.0584"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit effort="20" lower="0.0" upper="0.04" velocity="0.2"/>
|
||||
</joint>
|
||||
<joint name="panda_finger_joint2" type="prismatic">
|
||||
<parent link="panda_hand"/>
|
||||
<child link="panda_rightfinger"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.0584"/>
|
||||
<axis xyz="0 -1 0"/>
|
||||
<limit effort="20" lower="0.0" upper="0.04" velocity="0.2"/>
|
||||
<mimic joint="panda_finger_joint1"/>
|
||||
</joint>
|
||||
</robot>
|
||||
|
||||
@@ -7,7 +7,7 @@ Work is under progress for other simulators.
|
||||
import pyrobolearn as prl
|
||||
|
||||
sim = prl.simulators.Bullet()
|
||||
sim1 = prl.simulators.BulletROS()
|
||||
sim1 = prl.simulators.Mujoco()
|
||||
sim2 = prl.simulators.Dart()
|
||||
```
|
||||
|
||||
@@ -18,11 +18,14 @@ Check the `worlds` folder and the `robots` folder.
|
||||
#### TODOs
|
||||
|
||||
- [x] implement Bullet interface
|
||||
- [ ] implement BulletROS interface (ongoing)
|
||||
- [ ] implement Mujoco interface
|
||||
- [ ] implement Isaac interface
|
||||
- [ ] implement Dart interface
|
||||
- [ ] implement Mujoco interface (ongoing)
|
||||
- [ ] implement Raisim interface (ongoing)
|
||||
- [ ] implement Dart interface (ongoing)
|
||||
- [ ] implement ROS middleware (ongoing - currently worked with real franka emika panda robot)
|
||||
- [ ] implement RBDL_ROS interface
|
||||
- [ ] implement GazeboROS interface
|
||||
- [ ] implement V-REP (PyRep) interface
|
||||
- [ ] implement Isaac interface
|
||||
- [ ] implement Chrono interface
|
||||
- [ ] implement OpenSim interface (useful for biomedical models)
|
||||
- [ ] implement `simulator_randomizer` (similar to `physics_randomizer`)
|
||||
|
||||
@@ -7,3 +7,11 @@ them to send/receive messages. This allows to communicate with real platforms as
|
||||
|
||||
The Middleware has a list of RobotMiddleware, where each one specifies how to communicate with the robot middleware.
|
||||
|
||||
Note that this part is under construction, but we could already make it work with a real Franka Emika Panda robot
|
||||
arm. The code used for that are the 2 examples that are located in ``examples/middlewares/bullet_ros_control_gazebo.py``,
|
||||
and ``examples/imitation/demo.py``. The last example required to move the real robot, which automatically then
|
||||
moved the corresponding robot in the simulator where trajectory data was collected. A DMP was then trained on
|
||||
that data, and then the real robot was teleoperated from the simulator using the trained DMP. Note that if
|
||||
you are interested to implement your own robot middleware, check the ``franka.py`` file in the ``robots``
|
||||
subfolder.
|
||||
|
||||
|
||||
@@ -11,11 +11,7 @@ try:
|
||||
import rosmsg
|
||||
import rosservice
|
||||
import rostopic
|
||||
try:
|
||||
import controller_manager.controller_manager_interface as cm_interface
|
||||
except ImportError as e:
|
||||
print("ROS control is not installed for this Python version, please install it... For now, disabling the "
|
||||
"ROS control module... Calling methods that use the controller mananger will fail...")
|
||||
import controller_manager.controller_manager_interface as cm_interface
|
||||
|
||||
from .ros import ROS
|
||||
except ImportError as e:
|
||||
|
||||
@@ -59,9 +59,8 @@ class Middleware(object):
|
||||
# self.is_teleoperating = teleoperate
|
||||
# self.is_commanding = command
|
||||
self._subscribe, self._publish, self._teleoperate, self._command = False, False, False, False
|
||||
self.switch_mode(subscribe=subscribe, publish=publish, teleoperate=teleoperate, command=command)
|
||||
|
||||
self._robots = {} # {body_id: RobotMiddleware}
|
||||
self.switch_mode(subscribe=subscribe, publish=publish, teleoperate=teleoperate, command=command)
|
||||
|
||||
##############
|
||||
# Properties #
|
||||
@@ -194,6 +193,10 @@ class Middleware(object):
|
||||
self._teleoperate = bool(teleoperate)
|
||||
self._command = bool(command)
|
||||
|
||||
for robot in self._robots.values():
|
||||
robot.switch_mode(subscribe=self._subscribe, publish=self._publish, teleoperate=self._teleoperate,
|
||||
command=self._command)
|
||||
|
||||
def close(self):
|
||||
"""
|
||||
Close the middleware.
|
||||
|
||||
@@ -24,7 +24,7 @@ from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
|
||||
|
||||
MoveJoints = None
|
||||
try:
|
||||
from panda_arm.srv import MoveJoints
|
||||
from move_to_pose_iit.srv import MoveJoints
|
||||
except ImportError as e:
|
||||
print("The service `MoveJoints` is not available... Please compile it using `catkin_make` in order to use it "
|
||||
"when resetting the joint states.\n" + str(e))
|
||||
@@ -104,8 +104,8 @@ class FrankaROSMiddleware(ROSRobotMiddleware):
|
||||
# self.hand_point.effort = 0.1 * np.ones(2)
|
||||
|
||||
# update publisher
|
||||
arm_topic = '/panda_arm_controller/command'
|
||||
# arm_topic = '/position_joint_trajectory_controller/command'
|
||||
# arm_topic = '/panda_arm_controller/command'
|
||||
arm_topic = '/position_joint_trajectory_controller/command'
|
||||
self.arm_publisher = self.publisher.create_publisher(name='panda_arm_trajectory', topic=arm_topic,
|
||||
msg_class=JointTrajectory)
|
||||
self.use_hand = False
|
||||
@@ -123,7 +123,7 @@ class FrankaROSMiddleware(ROSRobotMiddleware):
|
||||
self.reset_joint_service = None
|
||||
self.use_real_robot = True
|
||||
if MoveJoints is not None and self.use_real_robot:
|
||||
self.reset_joint_service_name = '/arm/move_joint_absolute'
|
||||
self.reset_joint_service_name = '/arm/move_joints_absolute'
|
||||
self.reset_joint_service = rospy.ServiceProxy(self.reset_joint_service_name, MoveJoints)
|
||||
|
||||
self.filter = MovingAverageFilter(alpha=0.3)
|
||||
@@ -147,13 +147,14 @@ class FrankaROSMiddleware(ROSRobotMiddleware):
|
||||
# call rosservice to reset the joints
|
||||
rospy.wait_for_service(self.reset_joint_service_name)
|
||||
try:
|
||||
print("Resetting joint states on the real platform...")
|
||||
# keep only joint arm indices/positions
|
||||
q_indices = None if joint_ids is None else self.q_indices[joint_ids]
|
||||
if q_indices is None:
|
||||
q_indices = np.array(range(7))
|
||||
positions = positions[:7]
|
||||
q_indices = q_indices[q_indices <= 6]
|
||||
q_indices = q_indices[q_indices < 7]
|
||||
positions = positions[q_indices]
|
||||
args = np.array(['Joint' + str(i+1) for i in range(6)])
|
||||
args = np.array(['T' + str(i+1) for i in range(7)])
|
||||
kwargs = dict(zip(args[q_indices], positions))
|
||||
velocity_scale = 0.1 # 1 = max velocity, 0 = don't move
|
||||
duration_time = 10 # 10 secs
|
||||
@@ -199,7 +200,9 @@ class FrankaROSMiddleware(ROSRobotMiddleware):
|
||||
if not self.use_hand:
|
||||
q_indices = q_indices[q_indices <= 6] # only keep the 7 first joints (correspond to the arm)
|
||||
positions = self.subscriber.get_joint_positions(q_indices)
|
||||
return np.concatenate((positions, np.zeros(2)))
|
||||
if positions is not None and len(positions) > 0:
|
||||
return np.concatenate((positions, np.zeros(2)))
|
||||
return None
|
||||
return self.subscriber.get_joint_positions(q_indices)
|
||||
|
||||
def set_joint_positions(self, positions, joint_ids=None, velocities=None, kps=None, kds=None, forces=None):
|
||||
@@ -226,9 +229,9 @@ class FrankaROSMiddleware(ROSRobotMiddleware):
|
||||
q_indices = q_indices[q_indices <= 6] # only keep the 7 first joints (correspond to the arm)
|
||||
|
||||
if q_indices is not None:
|
||||
q[q_indices] = positions
|
||||
q[q_indices] = positions[:7]
|
||||
if velocities is not None:
|
||||
dq[q_indices] = velocities
|
||||
dq[q_indices] = velocities[:7]
|
||||
|
||||
self.arm_point.positions = q[:7]
|
||||
self.arm_point.velocities = dq[:7]
|
||||
|
||||
@@ -1,5 +1,5 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(panda_arm)
|
||||
project(move_to_pose_iit)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS rospy roscpp std_msgs genmsg message_generation)
|
||||
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>panda_arm</name>
|
||||
<name>move_to_pose_iit</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The panda arm package</description>
|
||||
|
||||
|
||||
@@ -1,16 +1,16 @@
|
||||
float64 Joint1
|
||||
float64 T1
|
||||
|
||||
float64 Joint2
|
||||
float64 T2
|
||||
|
||||
float64 Joint3
|
||||
float64 T3
|
||||
|
||||
float64 Joint4
|
||||
float64 T4
|
||||
|
||||
float64 Joint5
|
||||
float64 T5
|
||||
|
||||
float64 Joint6
|
||||
float64 T6
|
||||
|
||||
float64 Joint7
|
||||
float64 T7
|
||||
|
||||
float64 scale
|
||||
|
||||
|
||||
@@ -48,11 +48,7 @@ import roslaunch
|
||||
import rosparam
|
||||
import rosmsg, rosservice
|
||||
import rostopic
|
||||
try: # TODO: fix this!!
|
||||
import controller_manager.controller_manager_interface as cm_interface
|
||||
except ImportError as e:
|
||||
print("ROS control is not installed for this Python version, please install it... For now, disabling the "
|
||||
"ROS control module... Calling methods that use the controller mananger will fail...")
|
||||
import controller_manager.controller_manager_interface as cm_interface
|
||||
|
||||
import std_msgs.msg as std_msg
|
||||
import sensor_msgs.msg as sensor_msg
|
||||
@@ -645,7 +641,7 @@ class ROS(Middleware):
|
||||
self.remappers = {}
|
||||
self.models = []
|
||||
|
||||
self._robots = {} # {body_id: ROSRobotMiddleware}
|
||||
# self._robots = {} # {body_id: ROSRobotMiddleware}
|
||||
self.count_id = -1
|
||||
|
||||
# init roslaunch
|
||||
@@ -1438,6 +1434,7 @@ class ROS(Middleware):
|
||||
velocities (float, list[float], np.array[float]): the joint velocity(ies) (angular [rad/s] or linear
|
||||
velocity [m/s])
|
||||
"""
|
||||
|
||||
robot = self._robots.get(body_id)
|
||||
if robot is not None:
|
||||
return robot.reset_joint_states(positions, joint_ids=joint_ids, velocities=velocities)
|
||||
|
||||
@@ -171,7 +171,7 @@ class Body(object):
|
||||
self._v_idx0, self._v_idxf = 0, 0 # initial and final dq (velocity) indices
|
||||
self._q_idx1, self._v_idx1 = 0, 0 # initial q and dq indices (which don't take into account virtual joints)
|
||||
self._u_idx0, self._u_idxf = 0, 0 # initial and final ctrl indices
|
||||
self._u_p_indices = [] # ctrl indices for position motors (do +1 to get velocities, and +2 to get torques)
|
||||
self._u_p_indices = np.array([]) # ctrl ids for position motors (do +1 to get velocities, +2 to get torques)
|
||||
|
||||
# keep in memory the body
|
||||
# self.body = body
|
||||
@@ -186,14 +186,18 @@ class Body(object):
|
||||
else:
|
||||
jnt_to_q.append(idx)
|
||||
idx += 1
|
||||
self.jnt_to_q = np.array(jnt_to_q)
|
||||
self.jnt_to_q = np.array(jnt_to_q) # joint ids to q indices, e.g. [0, -1, -1, 1, 2, -1, 3] (-1 = fixed)
|
||||
|
||||
# keep in memory the link ids
|
||||
|
||||
# ctrl mode
|
||||
# define variables related to actuators and control
|
||||
self.ctrl_mode = struct.ControlMode.NULL # remember the last control mode
|
||||
self.gains = None # original gains
|
||||
self.biases = None # original biases
|
||||
self.ctrl_limited = None # original binary vector (nu,) to specify if the control inputs are limited
|
||||
self.force_limited = None # original binary vector (nu,) to specify if the forces are limited
|
||||
self.ctrl_range = None # original range of control inputs (nu, 2)
|
||||
self.force_range = None # original range of forces (nu, 2)
|
||||
|
||||
@property
|
||||
def num_links(self):
|
||||
@@ -377,8 +381,27 @@ class Body(object):
|
||||
"than 0!")
|
||||
self._u_p_indices = np.array(range(self._u_idx0, self._u_idxf, 3))
|
||||
|
||||
@property
|
||||
def u_p_indices(self):
|
||||
"""Return the ctrl indices for the position motors. To get the velocities, just add +1, and to get the efforts
|
||||
just add +2."""
|
||||
return self._u_p_indices
|
||||
|
||||
@property
|
||||
def u_v_indices(self):
|
||||
"""Return the ctrl indices for the velocity motors. To get the positions, just subtract 1, and to get the
|
||||
efforts just add 1."""
|
||||
return self._u_p_indices + 1
|
||||
|
||||
@property
|
||||
def u_e_indices(self):
|
||||
"""Return the ctrl indices for the effort (torque/force) motors. To get the positions, just subtract 2, and
|
||||
to get the velocities, subtract 1."""
|
||||
return self._u_p_indices + 2
|
||||
|
||||
@property
|
||||
def num_ctrl_inputs(self):
|
||||
"""Return the number of control inputs."""
|
||||
return self._u_idxf - self._u_idx0
|
||||
|
||||
@property
|
||||
@@ -393,16 +416,34 @@ class Body(object):
|
||||
return self.tag.attrib.get("name")
|
||||
|
||||
def get_q_idx(self, joint_id, keep=False):
|
||||
"""Return the q index(ices) associated with the given joint id(s).
|
||||
|
||||
Args:
|
||||
joint_id (np.array[int], int): joint id(s) (each joint id should be between [0, num_joints[).
|
||||
keep (bool): if True, keep the fixed joints.
|
||||
|
||||
Returns:
|
||||
np.array[int], int, None: q index(ices) associated with the given joint id(s).
|
||||
"""
|
||||
q = self.jnt_to_q[joint_id]
|
||||
if keep: # keep fixed joints (-1)
|
||||
return q
|
||||
if isinstance(q, float):
|
||||
if isinstance(q, int):
|
||||
if q != -1:
|
||||
return q
|
||||
return None
|
||||
return q[q != -1] # remove fixed joints
|
||||
|
||||
def get_dq_idx(self, joint_id, keep=False):
|
||||
"""Return the dq index(ices) associated with the given joint id(s).
|
||||
|
||||
Args:
|
||||
joint_id (np.array[int], int): joint id(s) (each joint id should be between [0, num_joints[).
|
||||
keep (bool): if True, keep the fixed joints.
|
||||
|
||||
Returns:
|
||||
np.array[int], int, None: dq index(ices) associated with the given joint id(s).
|
||||
"""
|
||||
return self.get_q_idx(joint_id, keep)
|
||||
|
||||
def get_joint(self, joint_id):
|
||||
@@ -664,6 +705,16 @@ class Mujoco(Simulator):
|
||||
|
||||
@staticmethod
|
||||
def _check_joint_id(body, joint_id):
|
||||
"""
|
||||
Check that the given joint_id are between [0, num_joints[.
|
||||
|
||||
Args:
|
||||
body (Body): MuJoCo body instance.
|
||||
joint_id (int): unique joint id.
|
||||
|
||||
Returns:
|
||||
int: joint id (same as the one given as input).
|
||||
"""
|
||||
if not isinstance(joint_id, int):
|
||||
raise TypeError("Expecting the given joint id to be an int, but got instead: {}".format(type(joint_id)))
|
||||
if joint_id < 0 or joint_id > (body.num_joints - 1):
|
||||
@@ -672,6 +723,16 @@ class Mujoco(Simulator):
|
||||
|
||||
@staticmethod
|
||||
def _check_joint_ids(body, joint_ids):
|
||||
"""
|
||||
Check that all the given joint ids are between [0, num_joints[.
|
||||
|
||||
Args:
|
||||
body (Body): MuJoCo body instance.
|
||||
joint_ids (np.array[int], int): unique joint ids.
|
||||
|
||||
Returns:
|
||||
np.array[int], int: joint id(s) (same as the ones given as inputs).
|
||||
"""
|
||||
joint_ids = np.asarray(joint_ids)
|
||||
if np.any(joint_ids < 0) or np.any(joint_ids > (body.num_joints - 1)):
|
||||
raise ValueError("joint_ids should belong to [0, {}], but got: {}".format(body.num_joints - 1, joint_ids))
|
||||
@@ -681,12 +742,16 @@ class Mujoco(Simulator):
|
||||
|
||||
@staticmethod
|
||||
def _check_link_id(body, link_id):
|
||||
"""Check the given link_id which should be between [-1, num_links-2], and return the converted link such that
|
||||
"""
|
||||
Check that the given link_id is between [-1, num_links-2], and return the converted link id such that
|
||||
it is between [0, num_links-1].
|
||||
|
||||
Args:
|
||||
body (Body): MuJoCo body instance.
|
||||
link_id (int): unique link id.
|
||||
|
||||
Returns:
|
||||
int: converted link id
|
||||
"""
|
||||
if not isinstance(link_id, int):
|
||||
raise TypeError("Expecting the given link id to be an int, but got instead: {}".format(type(link_id)))
|
||||
@@ -696,6 +761,17 @@ class Mujoco(Simulator):
|
||||
|
||||
@staticmethod
|
||||
def _check_link_ids(body, link_ids):
|
||||
"""
|
||||
Check that the given link_ids are between [-1, num_links-2], and return the converted link ids such that
|
||||
they are between [0, num_links-1].
|
||||
|
||||
Args:
|
||||
body (Body): MuJoCo body instance.
|
||||
link_ids (np.array[int], int): unique link id(s).
|
||||
|
||||
Returns:
|
||||
np.array[int], int: converted link id(s).
|
||||
"""
|
||||
link_ids = np.asarray(link_ids)
|
||||
if np.any(link_ids < -1) or np.any(link_ids > (body.num_bodies - 2)): # -1 is for the base
|
||||
raise ValueError("link_ids should belong to [-1, {}], but got: {}".format(body.num_bodies - 2, link_ids))
|
||||
@@ -705,7 +781,8 @@ class Mujoco(Simulator):
|
||||
|
||||
@staticmethod
|
||||
def _get_joint_type_id(joint_type):
|
||||
"""Return the joint type id given the joint type string.
|
||||
"""
|
||||
Return the joint type id given the joint type string.
|
||||
|
||||
Args:
|
||||
joint_type (str): joint type string.
|
||||
@@ -730,9 +807,17 @@ class Mujoco(Simulator):
|
||||
|
||||
@staticmethod
|
||||
def _process_name(name):
|
||||
"""Process name. By default, the MuJoCo parser add the prefix `prl_` and the suffix `_str(cnt)`. This is to
|
||||
avoid collisions between different names and making them unique. Here, we remove these prefix and suffix and
|
||||
return the original name (of the joint/body)."""
|
||||
"""
|
||||
Process the given name. By default, the MuJoCo parser add the prefix `prl_` and the suffix `_str(cnt)`.
|
||||
This is to avoid collisions between different names and making them unique. Here, we remove these prefix and
|
||||
suffix and return the original name (of the joint/body).
|
||||
|
||||
Args:
|
||||
name (str): name with a possible prefix `prl_` and suffix `_str(cnt)`.
|
||||
|
||||
Returns:
|
||||
str: processed name.
|
||||
"""
|
||||
if name.startswith('prl_'):
|
||||
return '_'.join(name.split('_')[1:-1])
|
||||
return name
|
||||
@@ -1290,6 +1375,14 @@ class Mujoco(Simulator):
|
||||
if verbose > 1:
|
||||
print(self._parser.get_string(pretty_format=True))
|
||||
|
||||
# save default gain and bias parameters, control inputs range, and force range
|
||||
# body.gains = self.model.actuator_gainprm[body.u_idx0:body.u_idxf]
|
||||
# body.biases = self.model.actuator_biasprm[body.u_idx0:body.u_idxf]
|
||||
# body.ctrl_limited = self.model.actuator_ctrllimited[body.u_idx0:body.u_idxf]
|
||||
# body.force_limited = self.model.actuator_forcelimited[body.u_idx0:body.u_idxf]
|
||||
# body.ctrl_range = self.model.actuator_ctrlrange[body.u_idx0:body.u_idxf]
|
||||
# body.force_range = self.model.actuator_forcerange[body.u_idx0:body.u_idxf]
|
||||
|
||||
# return body id
|
||||
return body_id
|
||||
|
||||
@@ -2708,10 +2801,8 @@ class Mujoco(Simulator):
|
||||
kds (None, float, np.array[float[N]]): velocity gain(s)
|
||||
forces (None, float, np.array[float[N]]): maximum motor force(s)/torque(s) used to reach the target values.
|
||||
"""
|
||||
# TODO: use the other arguments
|
||||
|
||||
body = self._bodies[body_id]
|
||||
|
||||
# body = self._bodies[body_id]
|
||||
#
|
||||
# q = self.get_joint_positions(body_id, joint_ids=joint_ids)
|
||||
# qvel= self.get_joint_velocities(body_id, joint_ids=joint_ids)
|
||||
#
|
||||
@@ -2750,22 +2841,48 @@ class Mujoco(Simulator):
|
||||
# # self.sim.data.qfrc_applied[body.v_idx1 + q[q != -1]] = tau + c_q_dq # DEPRECATED
|
||||
# self.sim.data.qfrc_actuator[body.v_idx1 + q[q != -1]] = tau + c_q_dq
|
||||
|
||||
# TODO: desactivate the other motors by setting their gains and biases to zero
|
||||
# TODO: use the given kps and kds
|
||||
body = self._bodies[body_id]
|
||||
|
||||
# check the current control mode, deactivate the other motors by setting their gains and biases to zero
|
||||
if velocities is None and body.ctrl_mode != struct.ControlMode.POSITION:
|
||||
pass
|
||||
# Switch to position control mode
|
||||
body_p_indices = body.u_p_indices - body.u_idx0
|
||||
self.model.actuator_gainprm[body.u_p_indices, 0] = body.gains[body_p_indices, 0]
|
||||
self.model.actuator_biasprm[body.u_p_indices, 1] = -body.biases[body_p_indices, 1]
|
||||
self.model.actuator_gainprm[body.u_v_indices] = 0
|
||||
self.model.actuator_biasprm[body.u_v_indices] = 0
|
||||
self.model.actuator_gainprm[body.u_e_indices] = 0
|
||||
self.model.actuator_biasprm[body.u_e_indices] = 0
|
||||
body.ctrl_mode = struct.ControlMode.POSITION
|
||||
elif velocities is not None and body.ctrl_mode != struct.ControlMode.PD:
|
||||
pass
|
||||
# Switch to PD control mode
|
||||
body_p_indices = body.u_p_indices - body.u_idx0
|
||||
body_v_indices = body_p_indices + 1
|
||||
self.model.actuator_gainprm[body.u_p_indices, 0] = body.gains[body_p_indices, 0]
|
||||
self.model.actuator_biasprm[body.u_p_indices, 1] = -body.biases[body_p_indices, 1]
|
||||
self.model.actuator_gainprm[body.u_v_indices, 0] = body.gains[body_v_indices, 0]
|
||||
self.model.actuator_biasprm[body.u_v_indices, 2] = -body.gains[body_v_indices, 2]
|
||||
self.model.actuator_gainprm[body.u_e_indices] = 0
|
||||
self.model.actuator_biasprm[body.u_e_indices] = 0
|
||||
body.ctrl_mode = struct.ControlMode.PD
|
||||
|
||||
if forces is not None:
|
||||
# self.model.actuator_forcelimited[] = 1
|
||||
# self.model.actuator_forcerange[] = None
|
||||
pass
|
||||
self.model.actuator_forcelimited[body.u_p_indices] = 1
|
||||
self.model.actuator_forcerange[body.u_p_indices, 0] = -forces
|
||||
self.model.actuator_forcerange[body.u_p_indices, 1] = forces
|
||||
# TODO: reset force range when None
|
||||
|
||||
if velocities is not None:
|
||||
self.model.actuator_ctrllimited[body.u_v_indices] = 1
|
||||
self.model.actuator_ctrlrange[body.u_v_indices, 0] = -velocities
|
||||
self.model.actuator_ctrlrange[body.u_v_indices, 1] = velocities
|
||||
# TODO: reset velocity range when None
|
||||
|
||||
if joint_ids is None:
|
||||
self.sim.data.ctrl[body.q_idx1:body.q_idxf] = positions
|
||||
self.sim.data.ctrl[body.u_p_indices] = positions
|
||||
if velocities is not None:
|
||||
# self.sim.data.ctrl[] = velocities
|
||||
pass
|
||||
self.sim.data.ctrl[body.u_v_indices] = velocities
|
||||
|
||||
else:
|
||||
# check if valid joints
|
||||
@@ -2773,13 +2890,17 @@ class Mujoco(Simulator):
|
||||
|
||||
# if one joint, set its position
|
||||
if isinstance(joint_ids, int):
|
||||
self.sim.data.ctrl[body.q_idx1 + joint_ids] = positions
|
||||
self.sim.data.ctrl[body.u_p_indices[joint_ids]] = positions
|
||||
if velocities is not None:
|
||||
# self.sim.data.ctrl[] = velocities
|
||||
pass
|
||||
self.sim.data.ctrl[body.u_v_indices[joint_ids]] = velocities
|
||||
|
||||
# if multiple joints, set their positions
|
||||
|
||||
else:
|
||||
q = body.get_q_idx(joint_ids, keep=True) # E.g. [0, -1, 1, -1, 2, 3] (-1 are for fixed joints)
|
||||
q = q[q != -1]
|
||||
self.sim.data.ctrl[body.u_p_indices + q] = positions
|
||||
if velocities is not None:
|
||||
self.sim.data.ctrl[body.u_v_indices + q] = velocities
|
||||
|
||||
def get_joint_positions(self, body_id, joint_ids=None):
|
||||
"""
|
||||
@@ -2824,24 +2945,58 @@ class Mujoco(Simulator):
|
||||
velocities (float, np.array[float[N]]): desired velocity, or list of desired velocities [rad/s]
|
||||
max_force (None, float, np.array[float[N]]): maximum motor forces/torques
|
||||
"""
|
||||
# TODO: use the other arguments
|
||||
# body = self._bodies[body_id]
|
||||
#
|
||||
# if joint_ids is None:
|
||||
# self.sim.data.qvel[body.v_idx1:body.v_idxf] = velocities
|
||||
# else:
|
||||
# # check if valid joints
|
||||
# self._check_joint_ids(body, joint_ids)
|
||||
#
|
||||
# # if one joint, set its velocity
|
||||
# if isinstance(joint_ids, int):
|
||||
# self.sim.data.qvel[body.v_idx1 + joint_ids] = velocities
|
||||
#
|
||||
# # if multiple joints, set their velocities
|
||||
# else:
|
||||
# q = body.get_q_idx(joint_ids, keep=True) # E.g. [0, -1, 1, -1, 2, 3] (-1 are for fixed joints)
|
||||
# self.sim.data.qvel[body.v_idx1 + q[q != -1]] = velocities
|
||||
|
||||
body = self._bodies[body_id]
|
||||
|
||||
# check the current control mode, deactivate the other motors by setting their gains and biases to zero
|
||||
if body.ctrl_mode != struct.ControlMode.VELOCITY:
|
||||
# Switch to velocity control mode
|
||||
body.ctrl_mode = struct.ControlMode.VELOCITY
|
||||
body_v_indices = body.u_v_indices - body.u_idx0
|
||||
self.model.actuator_gainprm[body.u_p_indices] = 0
|
||||
self.model.actuator_biasprm[body.u_p_indices] = 0
|
||||
self.model.actuator_gainprm[body.u_v_indices, 0] = body.gains[body_v_indices, 0]
|
||||
self.model.actuator_biasprm[body.u_v_indices, 2] = body.biases[body_v_indices, 2]
|
||||
self.model.actuator_gainprm[body.u_e_indices] = 0
|
||||
self.model.actuator_biasprm[body.u_e_indices] = 0
|
||||
|
||||
if max_force is not None:
|
||||
self.model.actuator_forcelimited[body.u_v_indices] = 1
|
||||
self.model.actuator_forcerange[body.u_v_indices, 0] = -max_force
|
||||
self.model.actuator_forcerange[body.u_v_indices, 1] = max_force
|
||||
# TODO: reset force range when None
|
||||
|
||||
if joint_ids is None:
|
||||
self.sim.data.qvel[body.v_idx1:body.v_idxf] = velocities
|
||||
self.sim.data.ctrl[body.u_v_indices] = velocities
|
||||
|
||||
else:
|
||||
# check if valid joints
|
||||
self._check_joint_ids(body, joint_ids)
|
||||
|
||||
# if one joint, set its velocity
|
||||
# if one joint, set its velocities
|
||||
if isinstance(joint_ids, int):
|
||||
self.sim.data.qvel[body.v_idx1 + joint_ids] = velocities
|
||||
self.sim.data.ctrl[body.u_v_indices[joint_ids]] = velocities
|
||||
|
||||
# if multiple joints, set their velocities
|
||||
else:
|
||||
q = body.get_q_idx(joint_ids, keep=True) # E.g. [0, -1, 1, -1, 2, 3] (-1 are for fixed joints)
|
||||
self.sim.data.qvel[body.v_idx1 + q[q != -1]] = velocities
|
||||
q_idx = body.get_q_idx(joint_ids, keep=True) # E.g. [0, -1, 1, -1, 2, 3] (-1 are for fixed joints)
|
||||
self.sim.data.ctrl[body.u_v_indices + q_idx[q_idx != -1]] = velocities
|
||||
|
||||
def get_joint_velocities(self, body_id, joint_ids=None):
|
||||
"""
|
||||
@@ -2915,9 +3070,9 @@ class Mujoco(Simulator):
|
||||
return self.sim.data.qacc[body.v_idx1 + joint_ids]
|
||||
|
||||
# if multiple joints, return their velocities
|
||||
q = body.get_q_idx(joint_ids, keep=True) # E.g. [0, -1, 1, -1, 2, 3] (-1 are for fixed joints)
|
||||
q_idx = body.get_q_idx(joint_ids, keep=True) # E.g. [0, -1, 1, -1, 2, 3] (-1 are for fixed joints)
|
||||
qacc = np.zeros(len(joint_ids))
|
||||
qacc[q != -1] = self.sim.data.qacc[body.v_idx1 + q[q != -1]]
|
||||
qacc[q_idx != -1] = self.sim.data.qacc[body.v_idx1 + q_idx[q_idx != -1]]
|
||||
return qacc
|
||||
|
||||
def set_joint_torques(self, body_id, joint_ids, torques):
|
||||
@@ -2929,22 +3084,64 @@ class Mujoco(Simulator):
|
||||
joint_ids (int, list[int]): joint id, or list of joint ids.
|
||||
torques (float, list[float], np.array[float]): desired torque(s) to apply to the joint(s) [N].
|
||||
"""
|
||||
# body = self._bodies[body_id]
|
||||
#
|
||||
# if joint_ids is None:
|
||||
# self.sim.data.qfrc_applied[body.v_idx1:body.v_idxf] = torques
|
||||
# else:
|
||||
# # check if valid joints
|
||||
# self._check_joint_ids(body, joint_ids)
|
||||
#
|
||||
# # if one joint, set its torque
|
||||
# if isinstance(joint_ids, int):
|
||||
# self.sim.data.qfrc_applied[body.v_idx1 + joint_ids] = torques
|
||||
#
|
||||
# # if multiple joints, set their torques
|
||||
# else:
|
||||
# q = body.get_q_idx(joint_ids, keep=True) # E.g. [0, -1, 1, -1, 2, 3] (-1 are for fixed joints)
|
||||
# self.sim.data.qfrc_applied[body.v_idx1 + q[q != -1]] = torques
|
||||
|
||||
body = self._bodies[body_id]
|
||||
|
||||
# check the current control mode, deactivate the other motors by setting their gains and biases to zero
|
||||
if body.ctrl_mode != struct.ControlMode.EFFORT:
|
||||
# Switch to effort control mode
|
||||
body.ctrl_mode = struct.ControlMode.EFFORT
|
||||
body_e_indices = body.u_e_indices - body.u_idx0
|
||||
self.model.actuator_gainprm[body.u_p_indices] = 0
|
||||
self.model.actuator_biasprm[body.u_p_indices] = 0
|
||||
self.model.actuator_gainprm[body.u_v_indices] = 0
|
||||
self.model.actuator_biasprm[body.u_v_indices] = 0
|
||||
self.model.actuator_gainprm[body.u_e_indices, 0] = body.gains[body_e_indices, 0]
|
||||
self.model.actuator_biasprm[body.u_e_indices] = 0
|
||||
|
||||
# reset to original control and force ranges
|
||||
self.model.actuator_ctrllimited[body.u_e_indices] = body.ctrl_limited[body_e_indices]
|
||||
self.model.actuator_ctrlrange[body.u_e_indices, 0] = body.ctrl_range[body_e_indices, 0]
|
||||
self.model.actuator_ctrlrange[body.u_e_indices, 1] = body.ctrl_range[body_e_indices, 1]
|
||||
self.model.actuator_forcelimited[body.u_e_indices] = body.force_limited[body_e_indices]
|
||||
self.model.actuator_forcerange[body.u_e_indices, 0] = body.force_range[body_e_indices, 0]
|
||||
self.model.actuator_forcerange[body.u_e_indices, 1] = body.force_range[body_e_indices, 1]
|
||||
|
||||
if joint_ids is None:
|
||||
self.sim.data.qfrc_applied[body.v_idx1:body.v_idxf] = torques
|
||||
self.sim.data.ctrl[body.u_e_indices] = torques
|
||||
|
||||
else:
|
||||
# check if valid joints
|
||||
self._check_joint_ids(body, joint_ids)
|
||||
|
||||
# if one joint, set its torque
|
||||
if isinstance(joint_ids, int):
|
||||
self.sim.data.qfrc_applied[body.v_idx1 + joint_ids] = torques
|
||||
# get q indices e.g. [0, -1, 1, -1, 2, 3] (-1 are for fixed joints)
|
||||
q_idx = body.get_q_idx(joint_ids, keep=True)
|
||||
|
||||
# if multiple joints, set their torques
|
||||
# if one joint, set its efforts (torques/forces)
|
||||
if isinstance(q_idx, int):
|
||||
if q_idx == -1:
|
||||
return
|
||||
self.sim.data.ctrl[body.u_e_indices[q_idx]] = torques
|
||||
|
||||
# if multiple joints, set their efforts (torques/forces)
|
||||
else:
|
||||
q = body.get_q_idx(joint_ids, keep=True) # E.g. [0, -1, 1, -1, 2, 3] (-1 are for fixed joints)
|
||||
self.sim.data.qfrc_applied[body.v_idx1 + q[q != -1]] = torques
|
||||
self.sim.data.ctrl[body.u_e_indices + q_idx[q_idx != -1]] = torques
|
||||
|
||||
def get_joint_torques(self, body_id, joint_ids=None):
|
||||
"""
|
||||
|
||||
@@ -1338,7 +1338,8 @@ class Simulator(object):
|
||||
|
||||
# publish the joint positions through the middleware
|
||||
if self.middleware is not None and self._middleware_enabled:
|
||||
self.middleware.reset_joint_states(body_id=body_id, joint_ids=joint_ids, positions=positions,
|
||||
middleware_id = self._middleware_ids[body_id]
|
||||
self.middleware.reset_joint_states(body_id=middleware_id, joint_ids=joint_ids, positions=positions,
|
||||
velocities=velocities)
|
||||
|
||||
def _reset_joint_states(self, body_id, joint_ids, positions, velocities=None):
|
||||
@@ -1836,7 +1837,8 @@ class Simulator(object):
|
||||
|
||||
# publish the joint positions through the middleware
|
||||
if self.middleware is not None and self._middleware_enabled:
|
||||
self.middleware.set_joint_positions(body_id, joint_ids, positions, velocities, kps, kds, forces)
|
||||
middleware_id = self._middleware_ids[body_id]
|
||||
self.middleware.set_joint_positions(middleware_id, joint_ids, positions, velocities, kps, kds, forces)
|
||||
|
||||
def _set_joint_positions(self, body_id, joint_ids, positions, velocities=None, kps=None, kds=None, forces=None):
|
||||
"""
|
||||
@@ -1919,7 +1921,8 @@ class Simulator(object):
|
||||
|
||||
# publish the joint velocities through the middleware
|
||||
if self.middleware is not None and self._middleware_enabled:
|
||||
self.middleware.set_joint_velocities(body_id, joint_ids, velocities, max_force)
|
||||
middleware_id = self._middleware_ids[body_id]
|
||||
self.middleware.set_joint_velocities(middleware_id, joint_ids, velocities, max_force)
|
||||
|
||||
def _set_joint_velocities(self, body_id, joint_ids, velocities, max_force=None):
|
||||
"""
|
||||
@@ -1949,10 +1952,16 @@ class Simulator(object):
|
||||
"""
|
||||
# if a middleware is defined
|
||||
if self.middleware is not None and self._middleware_enabled:
|
||||
middleware_id = self._middleware_ids[body_id]
|
||||
# get joint velocities from the middleware
|
||||
dq = self.middleware.get_joint_velocities(body_id, joint_ids)
|
||||
dq = self.middleware.get_joint_velocities(middleware_id, joint_ids)
|
||||
if dq is None: # if we didn't get the joint velocities from the middleware, get them from the simulator
|
||||
dq = self._get_joint_velocities(body_id, joint_ids)
|
||||
|
||||
# if the middleware is set on teleoperation mode, publish the joint positions through the middleware
|
||||
if self.middleware.is_teleoperating:
|
||||
self.middleware.set_joint_velocities(middleware_id, joint_ids, dq, check_teleoperate=True)
|
||||
|
||||
else: # if we got them from the middleware, set them in the simulator
|
||||
self._set_joint_velocities(body_id=body_id, joint_ids=joint_ids, velocities=dq)
|
||||
|
||||
@@ -1960,10 +1969,6 @@ class Simulator(object):
|
||||
# get the joint velocities from the simulator
|
||||
dq = self._get_joint_velocities(body_id, joint_ids)
|
||||
|
||||
# if the middleware is set on the teleoperation mode, publish the joint velocities through the middleware
|
||||
if self.middleware is not None and self._middleware_enabled:
|
||||
self.middleware.set_joint_velocities(body_id, joint_ids, dq, check_teleoperate=True)
|
||||
|
||||
return dq
|
||||
|
||||
def _get_joint_velocities(self, body_id, joint_ids):
|
||||
@@ -2025,7 +2030,8 @@ class Simulator(object):
|
||||
|
||||
# publish the joint torques through the middleware
|
||||
if self.middleware is not None and self._middleware_enabled:
|
||||
self.middleware.set_joint_torques(body_id, joint_ids, torques)
|
||||
middleware_id = self._middleware_ids[body_id]
|
||||
self.middleware.set_joint_torques(middleware_id, joint_ids, torques)
|
||||
|
||||
def _set_joint_torques(self, body_id, joint_ids, torques):
|
||||
"""
|
||||
@@ -2054,21 +2060,22 @@ class Simulator(object):
|
||||
"""
|
||||
# if a middleware is defined
|
||||
if self.middleware is not None and self._middleware_enabled:
|
||||
middleware_id = self._middleware_ids[body_id]
|
||||
# get joint torques from the middleware
|
||||
tau = self.middleware.get_joint_torques(body_id, joint_ids)
|
||||
tau = self.middleware.get_joint_torques(middleware_id, joint_ids)
|
||||
if tau is None: # if we didn't get the joint torques from the middleware, get them from the simulator
|
||||
tau = self._get_joint_torques(body_id, joint_ids)
|
||||
else: # if we got them from the middleware, set them in the simulator
|
||||
self._set_joint_torques(body_id=body_id, joint_ids=joint_ids, torques=tau)
|
||||
|
||||
# if the middleware is set on teleoperation mode, publish the joint positions through the middleware
|
||||
if self.middleware.is_teleoperating:
|
||||
self.middleware.set_joint_positions(middleware_id, joint_ids, tau, check_teleoperate=True)
|
||||
|
||||
else:
|
||||
# get the joint velocities from the simulator
|
||||
tau = self._get_joint_torques(body_id, joint_ids)
|
||||
|
||||
# if the middleware is set on the teleoperation mode, publish the joint torques through the middleware
|
||||
if self.middleware is not None and self._middleware_enabled:
|
||||
self.middleware.set_joint_torques(body_id, joint_ids, tau, check_teleoperate=True)
|
||||
|
||||
return tau
|
||||
|
||||
def _get_joint_torques(self, body_id, joint_ids):
|
||||
|
||||
@@ -1,4 +1,3 @@
|
||||
# -*- coding: utf-8 -*-
|
||||
#!/usr/bin/env python
|
||||
"""Define the `World` class which allows to specify what constitutes the world (i.e. what elements are in the world).
|
||||
|
||||
@@ -364,6 +363,13 @@ class World(object):
|
||||
self.sim.load(self.world_state)
|
||||
# reset the robot
|
||||
# self.reset_robots()
|
||||
|
||||
# reset the robots (we call this because sometimes we are using a robot middleware)
|
||||
for body_id, body in self.bodies.items():
|
||||
if isinstance(body, Robot):
|
||||
positions = body.get_joint_positions()
|
||||
velocities = body.get_joint_velocities()
|
||||
body.reset_joint_states(q=positions, dq=velocities)
|
||||
else:
|
||||
# reset simulation: remove all objects from the world and reset the world to initial conditions
|
||||
self.sim.reset()
|
||||
|
||||
@@ -73,3 +73,9 @@ nlopt>=2.4.2.post2
|
||||
trimesh>=3.1.1 # to compute volume, COM, inertia, etc.
|
||||
pyassimp>=4.1.4 # to import and export meshes in different formats
|
||||
# pymesh>=1.0.2
|
||||
|
||||
# install ros related packages
|
||||
rospkg>=1.1.7
|
||||
defusedxml>=0.4.1
|
||||
empy>=3.3.2
|
||||
# might need to install `ros_comm` and `ros_control` manually. Don't forget to switch the correct branch
|
||||
|
||||
Reference in New Issue
Block a user