diff --git a/pyrobolearn/robots/urdfs/franka/franka_mujoco.urdf b/pyrobolearn/robots/urdfs/franka/franka_mujoco.urdf
new file mode 100644
index 0000000..2f4e756
--- /dev/null
+++ b/pyrobolearn/robots/urdfs/franka/franka_mujoco.urdf
@@ -0,0 +1,340 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/pyrobolearn/simulators/README.md b/pyrobolearn/simulators/README.md
index 9752a81..f844621 100644
--- a/pyrobolearn/simulators/README.md
+++ b/pyrobolearn/simulators/README.md
@@ -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`)
diff --git a/pyrobolearn/simulators/middlewares/README.rst b/pyrobolearn/simulators/middlewares/README.rst
index a0287d8..4cebca1 100644
--- a/pyrobolearn/simulators/middlewares/README.rst
+++ b/pyrobolearn/simulators/middlewares/README.rst
@@ -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.
+
diff --git a/pyrobolearn/simulators/middlewares/__init__.py b/pyrobolearn/simulators/middlewares/__init__.py
index 0dbe33d..8c0835a 100644
--- a/pyrobolearn/simulators/middlewares/__init__.py
+++ b/pyrobolearn/simulators/middlewares/__init__.py
@@ -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:
diff --git a/pyrobolearn/simulators/middlewares/middleware.py b/pyrobolearn/simulators/middlewares/middleware.py
index 7799417..12dcc04 100644
--- a/pyrobolearn/simulators/middlewares/middleware.py
+++ b/pyrobolearn/simulators/middlewares/middleware.py
@@ -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.
diff --git a/pyrobolearn/simulators/middlewares/robots/franka.py b/pyrobolearn/simulators/middlewares/robots/franka.py
index 36ba46d..8a6baa5 100644
--- a/pyrobolearn/simulators/middlewares/robots/franka.py
+++ b/pyrobolearn/simulators/middlewares/robots/franka.py
@@ -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]
diff --git a/pyrobolearn/simulators/middlewares/robots/services/panda_arm/CMakeLists.txt b/pyrobolearn/simulators/middlewares/robots/services/panda_arm/CMakeLists.txt
index bffd187..4d4e888 100644
--- a/pyrobolearn/simulators/middlewares/robots/services/panda_arm/CMakeLists.txt
+++ b/pyrobolearn/simulators/middlewares/robots/services/panda_arm/CMakeLists.txt
@@ -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)
diff --git a/pyrobolearn/simulators/middlewares/robots/services/panda_arm/package.xml b/pyrobolearn/simulators/middlewares/robots/services/panda_arm/package.xml
index 0a33ae5..53ff131 100644
--- a/pyrobolearn/simulators/middlewares/robots/services/panda_arm/package.xml
+++ b/pyrobolearn/simulators/middlewares/robots/services/panda_arm/package.xml
@@ -1,6 +1,6 @@
- panda_arm
+ move_to_pose_iit0.0.0The panda arm package
diff --git a/pyrobolearn/simulators/middlewares/robots/services/panda_arm/srv/MoveJoints.srv b/pyrobolearn/simulators/middlewares/robots/services/panda_arm/srv/MoveJoints.srv
index 75b303d..e270208 100644
--- a/pyrobolearn/simulators/middlewares/robots/services/panda_arm/srv/MoveJoints.srv
+++ b/pyrobolearn/simulators/middlewares/robots/services/panda_arm/srv/MoveJoints.srv
@@ -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
diff --git a/pyrobolearn/simulators/middlewares/ros.py b/pyrobolearn/simulators/middlewares/ros.py
index 6d8ebb2..c18a568 100644
--- a/pyrobolearn/simulators/middlewares/ros.py
+++ b/pyrobolearn/simulators/middlewares/ros.py
@@ -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)
diff --git a/pyrobolearn/simulators/mujoco.py b/pyrobolearn/simulators/mujoco.py
index 9898171..8466b41 100644
--- a/pyrobolearn/simulators/mujoco.py
+++ b/pyrobolearn/simulators/mujoco.py
@@ -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):
"""
diff --git a/pyrobolearn/simulators/simulator.py b/pyrobolearn/simulators/simulator.py
index e2eda6d..4160831 100644
--- a/pyrobolearn/simulators/simulator.py
+++ b/pyrobolearn/simulators/simulator.py
@@ -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):
diff --git a/pyrobolearn/worlds/world.py b/pyrobolearn/worlds/world.py
index 38c0f9b..e5e4f5f 100644
--- a/pyrobolearn/worlds/world.py
+++ b/pyrobolearn/worlds/world.py
@@ -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()
diff --git a/requirements.txt b/requirements.txt
index f78f8f2..e568dde 100644
--- a/requirements.txt
+++ b/requirements.txt
@@ -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