wassname/pyrobolearn
1
0
Fork 0
mirror of https://github.com/wassname/pyrobolearn.git synced 2026-09-11 12:31:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

update parsers and simulators

Browse Source
This commit is contained in:
Brian Delhaisse
2019-09-12 15:19:32 +02:00
parent 32abb04ab2
commit 4e7fb22202
8 changed files with 883 additions and 234 deletions
Download Patch File Download Diff File Expand all files Collapse all files
pyrobolearn/simulators/bullet.py
+14 -9
View File
@@ -3323,7 +3323,8 @@ class Bullet(Simulator):
def change_dynamics(self, body_id, link_id=-1, mass=None, lateral_friction=None, spinning_friction=None,
rolling_friction=None, restitution=None, linear_damping=None, angular_damping=None,
contact_stiffness=None, contact_damping=None, friction_anchor=None,
local_inertia_diagonal=None, joint_damping=None):
local_inertia_diagonal=None, inertia_position=None, inertia_orientation=None,
joint_damping=None, joint_friction=None):
"""
Change dynamic properties of the given body (or link) such as mass, friction and restitution coefficients, etc.
@@ -3334,21 +3335,25 @@ class Bullet(Simulator):
lateral_friction (float): lateral (linear) contact friction
spinning_friction (float): torsional friction around the contact normal
rolling_friction (float): torsional friction orthogonal to contact normal
restitution (float): bouncyness of contact. Keep it a bit less than 1.
restitution (float): bounciness of contact. Keep it a bit less than 1.
linear_damping (float): linear damping of the link (0.04 by default)
angular_damping (float): angular damping of the link (0.04 by default)
contact_stiffness (float): stiffness of the contact constraints, used together with `contact_damping`
contact_damping (float): damping of the contact constraints for this body/link. Used together with
`contact_stiffness`. This overrides the value if it was specified in the URDF file in the contact
section.
`contact_stiffness`. This overrides the value if it was specified in the URDF file in the contact
section.
friction_anchor (int): enable or disable a friction anchor: positional friction correction (disabled by
default, unless set in the URDF contact section)
default, unless set in the URDF contact section)
local_inertia_diagonal (np.array[float[3]]): diagonal elements of the inertia tensor. Note that the base
and links are centered around the center of mass and aligned with the principal axes of inertia so
there are no off-diagonal elements in the inertia tensor.
and links are centered around the center of mass and aligned with the principal axes of inertia so
there are no off-diagonal elements in the inertia tensor.
inertia_position (np.array[float[3]]): new inertia position with respect to the link frame.
inertia_orientation (np.array[float[4]]): new inertia orientation (expressed as a quaternion [x,y,z,w]
with respect to the link frame.
joint_damping (float): joint damping coefficient applied at each joint. This coefficient is read from URDF
joint damping field. Keep the value close to 0.
`joint_damping_force = -damping_coefficient * joint_velocity`.
joint damping field. Keep the value close to 0.
`joint_damping_force = -damping_coefficient * joint_velocity`.
joint_friction (float): joint friction coefficient.
"""
kwargs = {}
if mass is not None:
pyrobolearn/simulators/dart.py
+234 -68
View File
@@ -303,7 +303,7 @@ class Dart(Simulator):
self.collision_shapes = {} # {collision_id: Collision}
self._bodies = OrderedDict() # {body_id: Body}
self.textures = {} # {texture_id: Texture}
self.constraints = OrderedDict() # {constraint_id: Constraint}
self._constraints = OrderedDict() # {constraint_id: Constraint}
# create counters
self._visual_cnt = 0
@@ -367,6 +367,74 @@ class Dart(Simulator):
# Methods #
###########
###########
# Private #
###########
@staticmethod
def _convert_wxyz_to_xyzw(q):
"""Convert a quaternion in the (w,x,y,z) format to (x,y,z,w)."""
return np.roll(q, shift=-1)
@staticmethod
def _convert_xyzw_to_wxyz(q):
"""Convert a quaternion in the (x,y,z,w) format to (w,x,y,z)."""
return np.roll(q, shift=1)
@staticmethod
def _get_matrix_from_transform(transform):
"""Return the homogeneous matrix from the given transform.
Args:
transform (dartpy.math.Isometry3): transform.
Returns:
np.array[float[4,4]]: homogeneous matrix.
"""
rot = transform.rotation()
pos = transform.translation()
return np.vstack((np.hstack((rot, pos.reshape(-1, 1))),
np.array([0., 0., 0., 1.])))
@staticmethod
def _get_pose_from_transform(transform):
"""Return the pose from the transform.
Args:
transform (dartpy.math.Isometry3): transform.
Returns:
np.array[float[3]]: position vector.
np.array[float[4]]: quaternion (expressed as [x,y,z,w]).
"""
quat = Dart._convert_wxyz_to_xyzw(transform.quaternion().wxyz())
pos = transform.translation()
return pos, quat
@staticmethod
def _get_quat_from_transform(transform):
"""Return the quaternion from the given transform.
Args:
transform (dartpy.math.Isometry3): transform.
Returns:
np.array[float[4]]: quaternion (expressed as [x,y,z,w]).
"""
return Dart._convert_wxyz_to_xyzw(transform.quaternion().wxyz())
@staticmethod
def _get_pos_from_transform(transform):
"""Return the position from the given transform.
Args:
transform (dartpy.math.Isometry3): transform.
Returns:
np.array[float[3]]: position vector.
"""
return transform.translation()
##############
# Simulators #
##############
@@ -569,13 +637,17 @@ class Dart(Simulator):
skeleton = self._urdf_parser.parseSkeleton(filename)
self.world.addSkeleton(skeleton)
rpy = get_rpy_from_quaternion(orientation) if orientation is not None else None
for i in range(6):
if i < 3:
if rpy is not None:
skeleton.setPosition(index=i, position=rpy[i])
else:
if position is not None:
skeleton.setPosition(index=i, position=position[i-3])
# set orientation
if rpy is not None:
for i in range(3):
skeleton.setPosition(index=i, position=rpy[i])
# set position
if position is not None:
for i in range(3):
skeleton.setPosition(index=i+3, position=position[i])
return self.world.getNumSkeletons() - 1
def load_sdf(self, filename, scaling=1., *args, **kwargs):
@@ -774,14 +846,12 @@ class Dart(Simulator):
dynamics_aspect = shape_node.createDynamicsAspect()
# set the position and orientation
if mass != 0:
orientation = get_rpy_from_quaternion(orientation) if orientation is not None else None
for i in range(6):
if i < 3:
if orientation is not None:
joint.setPosition(index=i, position=orientation[i])
else:
joint.setPosition(index=i, position=position[i-3])
orientation = get_rpy_from_quaternion(orientation) if orientation is not None else None
if orientation is not None:
for i in range(3):
joint.setPosition(i, orientation[i])
for i in range(3):
joint.setPosition(i+3, position[i])
# # increment body counter and remember the body
# self._body_cnt += 1
@@ -855,11 +925,13 @@ class Dart(Simulator):
def get_body_info(self, body_id):
"""Get the specified body information.
Specifically, it returns the base name extracted from the URDF, SDF, MJCF, or other file.
Args:
body_id (int): unique body id.
Returns:
dict, list: info
str: base name
"""
pass
@@ -872,7 +944,7 @@ class Dart(Simulator):
Returns:
int: unique body id.
"""
pass
return index
###############
# constraints #
@@ -909,7 +981,27 @@ class Dart(Simulator):
# ['BallJointConstraint', 'BoxedLcpConstraintSolver', 'BoxedLcpSolver', 'ConstraintBase', 'ConstraintSolver',
# 'DantzigBoxedLcpSolver', 'JointConstraint', 'JointCoulombFrictionConstraint', 'JointLimitConstraint',
# 'PgsBoxedLcpSolver', 'PgsBoxedLcpSolverOption', 'WeldJointConstraint']
pass
parent = self.world.getSkeleton(parent_body_id)
parent_node = parent.getBodyNode(parent_link_id + 1)
if child_body_id != -1: # if child is a skeleton
child = self.world.getSkeleton(child_body_id)
child_node = child.getBodyNode(child_link_id + 1)
else: # if child is the world
pass
# create constraint based on the type
if joint_type == Simulator.JOINT_FIXED:
constraint = dart.constraint.WeldJointConstraint(parent_node, child_node)
self._constraint_cnt += 1
self._constraints[self._constraint_cnt] = constraint
# add constraint
self.world.getConstraintSolver().addConstraint(constraint)
return self._constraint_cnt
def remove_constraint(self, constraint_id):
"""
@@ -918,7 +1010,8 @@ class Dart(Simulator):
Args:
constraint_id (int): constraint unique id.
"""
pass
constraint = self._constraints.pop(constraint_id)
self.world.getConstraintSolver().removeConstraint(constraint)
def change_constraint(self, constraint_id, *args, **kwargs):
"""
@@ -927,6 +1020,7 @@ class Dart(Simulator):
Args:
constraint_id (int): constraint unique id.
"""
constraint = self._constraints.pop(constraint_id)
pass
def num_constraints(self):
@@ -936,7 +1030,7 @@ class Dart(Simulator):
Returns:
int: number of constraints created.
"""
pass
return len(self._constraints)
def get_constraint_id(self, index):
"""
@@ -948,7 +1042,7 @@ class Dart(Simulator):
Returns:
int: constraint unique id.
"""
pass
return list(self._constraints.keys())[index]
def get_constraint_info(self, constraint_id):
"""
@@ -1028,18 +1122,18 @@ class Dart(Simulator):
skeleton = self.world.getSkeleton(body_id)
if link_ids is None:
com = skeleton.getCOM() # (3,1)
return com.reshape(-1)
return skeleton.getCOM()
# if isinstance(link_ids, int):
# link_ids = [link_ids]
#
# coms = []
# for link_id in link_ids:
# body = skeleton.getBodyNode(link_id + 1)
#
#
return None # TODO
if isinstance(link_ids, int):
return skeleton.getBodyNode(link_ids + 1).getCOM()
com = 0
for link_id in link_ids:
body = skeleton.getBodyNode(link_id + 1)
com += body.getCOM * body.getMass()
com /= skeleton.getMass()
return com
def get_center_of_mass_velocity(self, body_id, link_ids=None):
"""
@@ -1056,10 +1150,17 @@ class Dart(Simulator):
skeleton = self.world.getSkeleton(body_id)
if link_ids is None:
com_vel = skeleton.getCOMLinearVelocity() # (3,1)
return com_vel.reshape(-1)
return skeleton.getCOMLinearVelocity()
return None # TODO
if isinstance(link_ids, int):
return skeleton.getBodyNode(link_ids + 1).getCOMLinearVelocity()
vel = 0
for link_id in link_ids:
body = skeleton.getBodyNode(link_id + 1)
vel += body.getCOMLinearVelocity() * body.getMass()
vel /= skeleton.getMass()
return vel
def get_base_pose(self, body_id):
"""
@@ -1074,9 +1175,9 @@ class Dart(Simulator):
"""
base = self.world.getSkeleton(body_id).getRootBodyNode()
transform = base.getWorldTransform()
# position = transform[:-1, 3]
position = base.getCOM().reshape(-1)
orientation = get_quaternion_from_matrix(transform[:-1, :-1])
# position = self._get_pos_from_transform(transform)
position = base.getCOM()
orientation = self._get_quat_from_transform(transform)
return position, orientation
def get_base_position(self, body_id):
@@ -1090,8 +1191,8 @@ class Dart(Simulator):
np.array[float[3]]: base position.
"""
base = self.world.getSkeleton(body_id).getRootBodyNode()
# return base.getWorldTransform()[:-1, 3]
return base.getCOM().reshape(-1)
# return self._get_pos_from_transform(base.getWorldTransform())
return base.getCOM()
def get_base_orientation(self, body_id):
"""
@@ -1105,7 +1206,7 @@ class Dart(Simulator):
"""
base = self.world.getSkeleton(body_id).getRootBodyNode()
transform = base.getWorldTransform()
return get_quaternion_from_matrix(transform[:-1, :-1])
return self._get_quat_from_transform(transform)
def reset_base_pose(self, body_id, position, orientation):
"""
@@ -1116,7 +1217,13 @@ class Dart(Simulator):
position (np.array[float[3]]): new base position.
orientation (np.array[float[4]]): new base orientation (expressed as a quaternion [x,y,z,w])
"""
pass
skeleton = self.world.getSkeleton(body_id)
rpy = get_rpy_from_quaternion(orientation)
for i in range(6):
if i < 3:
skeleton.setPosition(index=i, position=rpy[i])
else:
skeleton.setPosition(index=i, position=position[i - 3])
def reset_base_position(self, body_id, position):
"""
@@ -1126,7 +1233,9 @@ class Dart(Simulator):
body_id (int): unique object id.
position (np.array[float[3]]): new base position.
"""
pass
skeleton = self.world.getSkeleton(body_id)
for i in range(3):
skeleton.setPosition(index=i+3, position=position[i])
def reset_base_orientation(self, body_id, orientation):
"""
@@ -1136,7 +1245,10 @@ class Dart(Simulator):
body_id (int): unique object id.
orientation (np.array[float[4]]): new base orientation (expressed as a quaternion [x,y,z,w])
"""
pass
skeleton = self.world.getSkeleton(body_id)
rpy = get_rpy_from_quaternion(orientation)
for i in range(3):
skeleton.setPosition(index=i, position=rpy[i])
def get_base_velocity(self, body_id):
"""
@@ -1191,7 +1303,9 @@ class Dart(Simulator):
linear_velocity (np.array[float[3]]): new linear velocity of the base.
angular_velocity (np.array[float[3]]): new angular velocity of the base.
"""
pass
skeleton = self.world.getSkeleton(body_id)
for i in range(3):
skeleton.setVelocity(index=i + 3, velocity=linear_velocity[i])
def reset_base_linear_velocity(self, body_id, linear_velocity):
"""
@@ -1201,7 +1315,9 @@ class Dart(Simulator):
body_id (int): unique object id.
linear_velocity (np.array[float[3]]): new linear velocity of the base
"""
pass
skeleton = self.world.getSkeleton(body_id)
for i in range(3):
skeleton.setVelocity(index=i+3, velocity=linear_velocity[i])
def reset_base_angular_velocity(self, body_id, angular_velocity):
"""
@@ -1211,7 +1327,9 @@ class Dart(Simulator):
body_id (int): unique object id.
angular_velocity (np.array[float[3]]): new angular velocity of the base
"""
pass
skeleton = self.world.getSkeleton(body_id)
for i in range(3):
skeleton.setVelocity(index=i, velocity=angular_velocity[i])
def apply_external_force(self, body_id, link_id=-1, force=(0., 0., 0.), position=(0., 0., 0.), frame=1):
"""
@@ -1229,7 +1347,7 @@ class Dart(Simulator):
link = self.world.getSkeleton(body_id).getBodyNode(link_id + 1)
force = np.asarray(force).reshape(-1, 1) # (3,1)
offset = np.asarray(position).reshape(-1, 1) # (3,1)
is_local = (frame == 1)
is_local = (frame == Simulator.LINK_FRAME)
link.setExtForce(force=force, offset=offset, isForceLocal=is_local, isOffsetLocal=is_local)
def apply_external_torque(self, body_id, link_id=-1, torque=(0., 0., 0.), frame=1):
@@ -1246,7 +1364,7 @@ class Dart(Simulator):
"""
link = self.world.getSkeleton(body_id).getBodyNode(link_id + 1)
torque = np.asarray(torque).reshape(-1, 1) # (3,1)
is_local = (frame == 1)
is_local = (frame == Simulator.LINK_FRAME)
link.setExtForce(torque=torque, isLocal=is_local)
###################
@@ -1331,7 +1449,40 @@ class Dart(Simulator):
[15] np.array[float[4]]: joint orientation in parent frame
[16] int: parent link index, -1 for base
"""
pass
skeleton = self.world.getSkeleton(body_id)
joint = skeleton.getJoint(joint_id + 1)
name = joint.getName()
if isinstance(joint, dart.dynamics.FreeJoint):
joint_type = Simulator.JOINT_FREE
elif isinstance(joint, dart.dynamics.WeldJoint):
joint_type = Simulator.JOINT_FIXED
elif isinstance(joint, dart.dynamics.RevoluteJoint):
joint_type = Simulator.JOINT_REVOLUTE
elif isinstance(joint, dart.dynamics.BallJoint):
joint_type = Simulator.JOINT_SPHERICAL
elif isinstance(joint, dart.dynamics.PrismaticJoint):
joint_type = Simulator.JOINT_PRISMATIC
else:
joint_type = -1
q_idx = joint.getIndexInTree(joint_id) # or joint.getIndexInSkeleton(joint_id)
dq_idx = 0
flag = -1
damping = joint.getDampingCoefficient()
friction = joint.getCoulombFriction()
pos_limits = (joint.getPositionLowerLimit, joint.getPositionUpperLimit)
force_limits = (joint.getForceLowerLimit, joint.getForceUpperLimit)
vel_limits = (joint.getVelocityLowerLimit, joint.getVelocityUpperLimit)
link_name = joint.getChildBodyNode().getName()
axis = joint.getAxis() if hasattr(joint, 'getAxis') else np.zeros(3)
transform = joint.getTransformFromParentBodyNode()
pos, quat = self._get_pose_from_transform(transform)
return joint_id, name, joint_type, q_idx, dq_idx, flag, damping, friction, pos_limits, force_limits, \
vel_limits, link_name, axis, pos, quat
def get_joint_state(self, body_id, joint_id):
"""
@@ -1351,9 +1502,10 @@ class Dart(Simulator):
is exactly what you provide, so there is no need to report it separately.
"""
joint = self.world.getSkeleton(body_id).getJoint(joint_id + 1)
body = joint.getChildBodyNode()
position = joint.getPosition(0) # joints can have less or more than 1 DoF (like WeldJoint, FreeJoint)
velocity = joint.getVelocity(0)
reaction_forces = np.zeros(6) # TODO
reaction_forces = body.getExternalForceGlobal()
torque = joint.getForce(0)
return position, velocity, reaction_forces, torque
@@ -1379,7 +1531,7 @@ class Dart(Simulator):
return self.get_joint_state(body_id, joint_ids)
return [self.get_joint_state(body_id, joint_id) for joint_id in joint_ids]
def reset_joint_state(self, body_id, joint_id, position, velocity=0.):
def reset_joint_state(self, body_id, joint_id, position, velocity=None):
"""
Reset the state of the joint. It is best only to do this at the start, while not running the simulation:
`reset_joint_state` overrides all physics simulation.
@@ -1408,7 +1560,7 @@ class Dart(Simulator):
"""
pass
def set_joint_motor_control(self, body_id, joint_ids, control_mode=2, positions=None,
def set_joint_motor_control(self, body_id, joint_ids, control_mode=Simulator.POSITION_CONTROL, positions=None,
velocities=None, forces=None, kp=None, kd=None, max_velocity=None):
r"""
Set the joint motor control.
@@ -2142,7 +2294,16 @@ class Dart(Simulator):
if multiple joints:
np.array[float[N,6]]: joint reaction forces [N, Nm]
"""
pass
if isinstance(joint_ids, int):
joint = self.world.getSkeleton(body_id).getJoint(joint_ids + 1)
body = joint.getChildBodyNode()
return body.getExternalForceGlobal()
forces = []
for joint_id in joint_ids:
joint = self.world.getSkeleton(body_id).getJoint(joint_id + 1)
body = joint.getChildBodyNode()
forces.append(body.getExternalForceGlobal())
return np.array(forces)
def get_joint_powers(self, body_id, joint_ids):
"""Return the applied power at the given joint(s). Power = torque * velocity.
@@ -2709,13 +2870,14 @@ class Dart(Simulator):
damping = -1
stiffness = -1
return [mass, friction, local_inertia_diag, position, orientation, restitution, rolling_friction,
spinning_friction, damping, stiffness]
return mass, friction, local_inertia_diag, position, orientation, restitution, rolling_friction, \
spinning_friction, damping, stiffness
def change_dynamics(self, body_id, link_id=-1, mass=None, lateral_friction=None, spinning_friction=None,
rolling_friction=None, restitution=None, linear_damping=None, angular_damping=None,
contact_stiffness=None, contact_damping=None, friction_anchor=None,
local_inertia_diagonal=None, joint_damping=None):
local_inertia_diagonal=None, inertia_position=None, inertia_orientation=None,
joint_damping=None, joint_friction=None):
"""
Change dynamic properties of the given body (or link) such as mass, friction and restitution coefficients, etc.
@@ -2726,21 +2888,25 @@ class Dart(Simulator):
lateral_friction (float): lateral (linear) contact friction
spinning_friction (float): torsional friction around the contact normal
rolling_friction (float): torsional friction orthogonal to contact normal
restitution (float): bouncyness of contact. Keep it a bit less than 1.
restitution (float): bounciness of contact. Keep it a bit less than 1.
linear_damping (float): linear damping of the link (0.04 by default)
angular_damping (float): angular damping of the link (0.04 by default)
contact_stiffness (float): stiffness of the contact constraints, used together with `contact_damping`
contact_damping (float): damping of the contact constraints for this body/link. Used together with
`contact_stiffness`. This overrides the value if it was specified in the URDF file in the contact
section.
`contact_stiffness`. This overrides the value if it was specified in the URDF file in the contact
section.
friction_anchor (int): enable or disable a friction anchor: positional friction correction (disabled by
default, unless set in the URDF contact section)
default, unless set in the URDF contact section)
local_inertia_diagonal (np.array[float[3]]): diagonal elements of the inertia tensor. Note that the base
and links are centered around the center of mass and aligned with the principal axes of inertia so
there are no off-diagonal elements in the inertia tensor.
and links are centered around the center of mass and aligned with the principal axes of inertia so
there are no off-diagonal elements in the inertia tensor.
inertia_position (np.array[float[3]]): new inertia position with respect to the link frame.
inertia_orientation (np.array[float[4]]): new inertia orientation (expressed as a quaternion [x,y,z,w]
with respect to the link frame.
joint_damping (float): joint damping coefficient applied at each joint. This coefficient is read from URDF
joint damping field. Keep the value close to 0.
`joint_damping_force = -damping_coefficient * joint_velocity`.
joint damping field. Keep the value close to 0.
`joint_damping_force = -damping_coefficient * joint_velocity`.
joint_friction (float): joint friction coefficient.
"""
skeleton = self.world.getSkeleton(body_id)
body = skeleton.getBodyNode(link_id + 1)
@@ -2764,8 +2930,8 @@ class Dart(Simulator):
if joint_damping is not None:
joint.setDampingCoefficient(joint_damping)
# if joint_friction is not None:
# joint.setCoulombFriction(joint_friction)
if joint_friction is not None:
joint.setCoulombFriction(joint_friction)
def calculate_jacobian(self, body_id, link_id, local_position, q, dq=None, des_ddq=None):
r"""
pyrobolearn/simulators/mujoco.py
+480 -135
View File
@@ -77,6 +77,9 @@ from pyrobolearn.simulators.simulator import Simulator
from pyrobolearn.utils.parsers.robots import URDFParser, MuJoCoParser, SDFParser
import pyrobolearn.utils.parsers.robots.data_structures as struct
from pyrobolearn.utils.transformation import get_homogeneous_matrix, get_quaternion_from_matrix
from pyrobolearn.utils.parsers.robots.data_structures import transform_child_joint_frame_to_parent_inertial_frame, \
transform_inertial_frame_to_child_inertial_frame, transform_inertial_frame_to_child_link_frame, \
transform_inertial_frame_to_joint_frame
# check Python version
@@ -127,7 +130,7 @@ class Body(object):
# check given body
if body is None:
body = struct.MultiBody()
body = struct.MultiBody(name='prl_dummy_' + str(body_id), root=struct.Body(body_id=body_id))
if not isinstance(body, struct.MultiBody):
raise TypeError("Expecting the given 'body' to be an instance of `MultiBody` but got instead: "
"{}".format(type(body)))
@@ -148,6 +151,7 @@ class Body(object):
self._b_idx0, self._b_idxf = 0, 0 # initial and final body indices
self._j_idx0, self._j_idxf = 0, 0 # initial and final free joint indices
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)
# keep in memory the body
# self.body = body
@@ -164,6 +168,9 @@ class Body(object):
idx += 1
self.jnt_to_q = np.array(jnt_to_q)
# keep in memory the link ids
@property
def num_links(self):
"""Alias to `num_bodies`."""
@@ -182,6 +189,12 @@ class Body(object):
raise ValueError("Error while setting the initial q index, this index has to be bigger than 0!")
self._q_idx0 = q
self._q_idxf = q + self.q_length # set the final q index
self._q_idx1 = q if self.fixed else q + 7 # set the initial q index (doesn't take into account virtual joints)
@property
def q_idx1(self):
"""Return the initial q index that does not take into account the virtual joints for the base."""
return self._q_idx1
@property
def q_idxf(self):
@@ -277,6 +290,12 @@ class Body(object):
raise ValueError("Error while setting the initial velocity index, this index has to be bigger than 0!")
self._v_idx0 = v
self._v_idxf = v + self.num_dofs # set the final velocity index
self._v_idx1 = v if self.fixed else v + 6 # set the initial q index (doesn't take into account virtual joints)
@property
def v_idx1(self):
"""Return the initial velocity index that does not take into account the virtual joints for the base."""
return self._v_idx1
@property
def v_idxf(self):
@@ -312,25 +331,43 @@ class Body(object):
if keep: # keep fixed joints (-1)
return q
if isinstance(q, float):
if q!=-1:
if q != -1:
return q
return []
return q[q!=-1] # remove fixed joints
return q[q != -1] # remove fixed joints
def get_dq_idx(self, joint_id, keep=False):
return self.get_q_idx(joint_id, keep)
def check_joint_id(self, joint_id):
if joint_id < 0 or joint_id > (self.num_joints - 1):
raise ValueError("joint_id should belong to [0, `num_joints-1`].")
def check_link_id(self, link_id):
if link_id < -1 or link_id > (self.num_bodies - 2): # -1 is for the base
raise ValueError("link_id should belong to [-1, `num_links-2`].")
def get_joint(self, joint_id):
return self.joints[joint_id]
def get_joint_type(self, joint_id):
return self.joints[joint_id].dtype
def get_link(self, link_id):
return self.links[link_id]
def transform_inertial_frame_to_joint_frame(self, body_id):
"""Return the homogeneous transform from the inertial frame to the joint frame."""
body = self.links[body_id]
return transform_inertial_frame_to_joint_frame(body)
def transform_child_joint_frame_to_parent_inertial_frame(self, child_body_id):
"""Return the homogeneous transform from the child joint frame to the parent inertial frame."""
body = self.links[child_body_id]
return transform_child_joint_frame_to_parent_inertial_frame(body)
def transform_inertial_frame_to_child_link_frame(self, child_body_id):
"""Return the homogeneous transform from the parent inertial frame to the child link/joint frame."""
body = self.links[child_body_id]
return transform_inertial_frame_to_child_link_frame(body)
def transform_inertial_frame_to_child_inertial_frame(self, child_body_id):
"""Return the homogeneous transform from the parent inertial frame to the child inertial frame."""
body = self.links[child_body_id]
return transform_inertial_frame_to_child_inertial_frame(body)
class Mujoco(Simulator):
r"""Mujoco Simulator interface.
@@ -507,6 +544,8 @@ class Mujoco(Simulator):
If None, it will take the root defined in the simulator.
render (bool): if we should render or not.
"""
# self.render(enable=False) # to delete the previous viewer instance if defined
# create the model
# self.model = mujoco.load_model_from_path(path)
root = self._parser.get_string(pretty_format=False)
@@ -517,7 +556,6 @@ class Mujoco(Simulator):
# if we need to render
if render:
# self.render(enable=False) # to delete the previous viewer instance if defined
# self.render(enable=True) # to instantiate the viewer
if self.viewer is None:
self.render(enable=True)
@@ -525,6 +563,63 @@ class Mujoco(Simulator):
print("Update the viewer's sim")
self.viewer.update_sim(self.sim)
@staticmethod
def _check_joint_id(body, joint_id):
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):
raise ValueError("joint_id should belong to [0, {}], but got: {}".format(body.num_joints - 1, joint_id))
return joint_id
@staticmethod
def _check_joint_ids(body, joint_ids):
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))
if joint_ids.ndim == 0:
return int(joint_ids)
return joint_ids
@staticmethod
def _check_link_id(body, 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)))
if link_id < -1 or link_id > (body.num_bodies - 2): # -1 is for the base
raise ValueError("link_id should belong to [-1, {}], but got: {}".format(body.num_bodies - 2, link_id))
return link_id + 1 # shift such that between [0, `body.num_bodies`[.
@staticmethod
def _check_link_ids(body, link_ids):
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))
if link_ids.ndim == 0:
return int(link_ids) + 1
return link_ids + 1 # shift such that between [0, `body.num_bodies`[.
@staticmethod
def _get_joint_type_id(joint_type):
if joint_type == 'fixed':
return Simulator.JOINT_FIXED
if joint_type == 'revolute':
return Simulator.JOINT_REVOLUTE
if joint_type == 'prismatic':
return Simulator.JOINT_PRISMATIC
elif joint_type == 'ball':
return Simulator.JOINT_SPHERICAL
elif joint_type == 'floating':
return Simulator.JOINT_FREE
elif joint_type == 'gear':
return Simulator.JOINT_GEAR
else:
return -1
@staticmethod
def _process_name(name):
if name.startswith('prl_'):
return '_'.join(name.split('_')[1:-1])
return name
#################
# utils methods #
#################
@@ -532,12 +627,14 @@ class Mujoco(Simulator):
@staticmethod
def _convert_wxyz_to_xyzw(q):
"""Convert a quaternion in the (w,x,y,z) format to (x,y,z,w)."""
return np.roll(q, shift=-1)
q = np.asarray(q)
return np.roll(q, shift=-1, axis=q.ndim - 1)
@staticmethod
def _convert_xyzw_to_wxyz(q):
"""Convert a quaternion in the (x,y,z,w) format to (w,x,y,z)."""
return np.roll(q, shift=1)
q = np.asarray(q)
return np.roll(q, shift=1, axis=q.ndim - 1)
##############
# Simulators #
@@ -742,7 +839,7 @@ class Mujoco(Simulator):
body.add_parent_joint(joint)
tree.add_joint(joint, idx=0)
return self._create_body(tree, verbose=2)
return self._create_body(tree, verbose=1)
def load_sdf(self, filename, scaling=1., *args, **kwargs): # TODO
"""Load a SDF file in the simulator.
@@ -870,9 +967,10 @@ class Mujoco(Simulator):
self._create_sim(render=self._render)
else:
# notify that the Mujoco model has changed (this will be checked in the `step` method)
self._model_changed = True
# self._model_changed = True
self._create_sim()
def _create_body(self, tree, body_id=None, verbose=2):
def _create_body(self, tree, body_id=None, verbose=1):
"""
Create inner body in the simulator; given the tree data structure, it generates all the XML tags using
the inner parser/generator, wraps the returned tree XML tag to a Body structure, sets its attributes (such as
@@ -972,7 +1070,7 @@ class Mujoco(Simulator):
if not static:
tree.add_joint(joint)
return self._create_body(tree, body_id=self._body_cnt, verbose=2)
return self._create_body(tree, body_id=self._body_cnt, verbose=1)
def remove_body(self, body_id): # DONE
"""Remove a particular body in the simulator.
@@ -1538,7 +1636,7 @@ class Mujoco(Simulator):
Returns:
int: number of links with the associated body id.
"""
return self._bodies[body_id].num_links
return self._bodies[body_id].num_links - 1 # remove the base link
def get_joint_info(self, body_id, joint_id):
"""
@@ -1576,33 +1674,67 @@ class Mujoco(Simulator):
"""
body = self._bodies[body_id]
if joint_id < 0 or joint_id > (body.num_joints - 1):
raise ValueError("joint_id should belong to [0, `num_joints-1`].")
# check joint id
joint_id = self._check_joint_id(body, joint_id)
joint = body.get_joint(joint_id)
dtype = body.get_joint_type(joint_id)
if dtype == 'fixed': # special care for fixed joints (as they are usually not specified in Mujoco models)
pass
idx = body.j_idx0 + joint_id
name = self._process_name(joint.name)
dtype = self._get_joint_type_id(joint.dtype)
name = self.model.joint_id2name()
dtype = self.model.jnt_type # ['free', 'ball', 'slide', 'hinge']
q_idx = body.get_q_idx(joint_id)
dq_idx = body.get_dq_idx(joint_id)
flag = -1
damping = self.model.dof_damping
friction = self.model.dof_frictionloss
limited = self.model.limited
limits = self.model.jnt_range
axis = self.model.jnt_axis
pos = self.model.jnt_pos
orientation = 0
# get index
q = body.get_q_idx(joint_id, keep=True) # -1 for fixed joint
flag = 1 # as in Bullet
if joint_id == body.num_joints - 1:
flag = 0
axis_pos = self.sim.data.xaxis
# xanchor
if q == -1: # fixed joint
q_idx = -1
dq_idx = -1
damping = 0.
friction = 0.
axis = np.zeros(3)
lower, upper = 0., -1.
max_vel = 0.
max_force = 0.
# stiffness
else:
joint_idx = body.j_idx0 + q
q_idx = q
dq_idx = q
# if not body.fixed: # compatible with Bullet
q_idx += 7
dq_idx += 6
dof_addr = self.model.jnt_dofadr[joint_idx]
damping = self.model.dof_damping[dof_addr]
friction = self.model.dof_frictionloss[dof_addr]
axis = self.model.jnt_axis[joint_idx]
limited = self.model.jnt_limited[joint_idx]
if limited:
lower, upper = self.model.jnt_range[joint_idx]
else:
lower, upper = -np.infty, np.infty
max_vel = joint.velocity
max_force = joint.effort
return joint_id
link = joint.child
link_name = link.name
parent_idx = joint.parent.id - 1 # the base is -1 by convention in Bullet
# position = joint.position # self.model.jnt_pos[body.j_idx0 + q]
# orientation = joint.quaternion
homogeneous = transform_inertial_frame_to_child_link_frame(link) # Bullet express joint from inertial
position = homogeneous[:3, 3]
orientation = get_quaternion_from_matrix(homogeneous[:3, :3])
if position is None:
position = np.zeros(3)
if orientation is None:
orientation = np.zeros(3)
# xanchor, stiffness, etc
return joint_id, name, dtype, q_idx, dq_idx, flag, damping, friction, lower, upper, max_force, max_vel, \
link_name, axis, position, orientation, parent_idx
def get_joint_state(self, body_id, joint_id):
"""
@@ -1627,19 +1759,24 @@ class Mujoco(Simulator):
if joint_id < 0 or joint_id > (body.num_joints - 1):
raise ValueError("joint_id should belong to [0, `num_joints-1`].")
q = body.get_q_idx(joint_id, keep=True)
if q == -1:
if q == -1: # fixed joint
return 0, 0, np.zeros(6), 0
qpos_idx = body.q_idx0 + q
qvel_idx = body.v_idx0 + q
if not body.fixed:
qpos_idx += 7
qvel_idx += 6
pos = self.sim.data.qpos[qpos_idx]
vel = self.sim.data.qvel[qvel_idx]
reaction_forces = np.zeros(6)
torque = self.sim.data.qfrc_applied[qvel_idx]
return pos, vel, reaction_forces, torque
# compute joint position and velocity
pos = self.sim.data.qpos[body.q_idx1 + q]
vel = self.sim.data.qvel[body.v_idx1 + q]
# compute reaction force
force_parent = self.sim.data.cfrc_int[body.b_idx0 + joint_id] # com-based interaction force with parent
force_ext = self.sim.data.cfrc_ext[body.b_idx0 + joint_id] # com-based external force on body
force = force_ext - force_parent # TODO: is it + instead of -?
np.roll(force, shift=3, axis=force.ndim - 1) # [torque, force] --> [force, torque]
# TODO: express it in the joint frame
# compute the applied torque
torque = self.sim.data.qfrc_applied[body.v_idx1 + q]
return pos, vel, force, torque
def get_joint_states(self, body_id, joint_ids):
"""
@@ -1661,7 +1798,7 @@ class Mujoco(Simulator):
"""
return [self.get_joint_state(body_id, joint_id) for joint_id in joint_ids]
def reset_joint_state(self, body_id, joint_id, position, velocity=0.):
def reset_joint_state(self, body_id, joint_id, position, velocity=None):
"""
Reset the state of the joint. It is best only to do this at the start, while not running the simulation:
`reset_joint_state` overrides all physics simulation.
@@ -1678,16 +1815,9 @@ class Mujoco(Simulator):
if joint_id < 0 or joint_id > (body.num_joints - 1):
raise ValueError("joint_id should belong to [0, `num_joints-1`].")
q = body.get_q_idx(joint_id, keep=True)
if q == -1:
return
qpos_idx = body.q_idx0 + q
qvel_idx = body.v_idx0 + q
if not body.fixed:
qpos_idx += 7
qvel_idx += 6
self.sim.data.qpos[qpos_idx] = position
self.sim.data.qvel[qvel_idx] = velocity
if q != -1:
self.sim.data.qpos[body.q_idx1 + q] = position
self.sim.data.qvel[body.v_idx1 + q] = velocity
def enable_joint_force_torque_sensor(self, body_id, joint_ids, enable=True):
"""
@@ -1698,9 +1828,12 @@ class Mujoco(Simulator):
joint_ids (int, int[N]): joint index in range [0..num_joints(body_id)], or list of joint ids.
enable (bool): True to enable, False to disable the force/torque sensor
"""
pass # TODO
# attach a force sensor to the specified body (site)
# attach a torque sensor to the specfied body (site)
# or check cfrc_int and cfrc_ext
pass
def set_joint_motor_control(self, body_id, joint_ids, control_mode=2, positions=None,
def set_joint_motor_control(self, body_id, joint_ids, control_mode=Simulator.POSITION_CONTROL, positions=None,
velocities=None, forces=None, kp=None, kd=None, max_velocity=None):
r"""
Set the joint motor control.
@@ -1757,22 +1890,28 @@ class Mujoco(Simulator):
[7] np.array[float[3]]: Cartesian world angular velocity. Only returned if `compute_velocity` is True.
"""
body = self._bodies[body_id]
if link_id < -1 or link_id > (body.num_bodies - 2): # -1 is for the base
raise ValueError("link_id should belong to [-1, `num_links-2`].")
idx = body.b_idx0 + 1 + link_id
link_id = self._check_link_id(body, link_id)
idx = body.b_idx0 + link_id
pos = self.sim.data.body_xpos[idx] # Cartesian position of body frame (same as xipos)
pos = self.sim.data.xipos[idx] # Cartesian position of body CoM
quat = self._convert_wxyz_to_xyzw(self.sim.data.body_xquat[idx]) # Cartesian orientation of body frame
# Note that in Mujoco the body frame is defined at the CoM
# link frame in URDF is the joint frame in Mujoco
# TODO: read from the Tree data structure and perform the correct transformations
link = body.get_link(link_id)
inertial = link.inertial
ipos = inertial.position if inertial is not None else np.zeros(3)
iquat = inertial.quaternion if inertial is not None else np.array([0., 0., 0., 1.])
lpos = self.sim.data.body_xpos[idx] # Cartesian position of body frame
lquat = quat
if compute_velocity:
vel = self.sim.data.cvel[idx] # com-based velocity [3D rot; 3D tran]
return pos, quat, vel[3:], vel[:3]
lin_vel, ang_vel = vel[3:], vel[:3]
else:
lin_vel, ang_vel = np.zeros(3), np.zeros(3)
return pos, quat
return pos, quat, ipos, iquat, lpos, lquat, lin_vel, ang_vel
def get_link_states(self, body_id, link_ids, compute_velocity=False, compute_forward_kinematics=False):
"""
@@ -1801,7 +1940,7 @@ class Mujoco(Simulator):
return [self.get_link_state(body_id, link_id, compute_velocity, compute_forward_kinematics)
for link_id in link_ids]
def get_link_names(self, body_id, link_ids):
def get_link_names(self, body_id, link_ids=None):
"""
Return the name of the given link(s).
@@ -1815,29 +1954,36 @@ class Mujoco(Simulator):
if multiple links:
str[N]: link names
"""
pass
body = self._bodies[body_id]
one_link = isinstance(link_ids, int)
if link_ids is None:
link_ids = range(0, body.num_bodies-1) # do not return the name of the base link
elif one_link:
link_ids = [link_ids]
names = []
for link_id in link_ids:
link_id = self._check_link_id(body, link_id)
name = self._process_name(body.get_link(link_id).name)
names.append(name)
if one_link:
return names[0]
return names
def _get_link_result(self, body_id, link_ids, mujoco_data, fct=None, slice=None):
body = self._bodies[body_id] # TODO: maybe use the tree data structure instead...
one_link = isinstance(link_ids, int)
if one_link:
link_ids = [link_ids]
results = []
for link_id in link_ids:
if link_id < -1 or link_id > (body.num_bodies - 2): # -1 is for the base
raise ValueError("link_id should belong to [-1, `num_links-2`].")
idx = body.b_idx0 + 1 + link_id
if link_ids is None:
data = mujoco_data[body.b_idx0+1:body.b_idxf] # +1 to not account for the base as in PyBullet
else:
link_ids = self._check_link_ids(body, link_ids)
idx = body.b_idx0 + link_ids
data = mujoco_data[idx]
if slice is not None:
data = data[slice]
if fct is not None:
data = fct(data)
results.append(data)
if one_link and len(results) > 0:
return results[0]
return results
if slice is not None:
data = data[slice]
if fct is not None:
data = fct(data)
return data
def get_link_masses(self, body_id, link_ids):
def get_link_masses(self, body_id, link_ids=None):
"""
Return the mass of the given link(s).
@@ -1851,10 +1997,9 @@ class Mujoco(Simulator):
else:
float[N]: mass of each link
"""
# TODO: maybe use the tree data structure instead...
return self._get_link_result(body_id, link_ids, self.sim.model.body_mass)
def get_link_frames(self, body_id, link_ids):
def get_link_frames(self, body_id, link_ids=None):
r"""
Return the link world frame position(s) and orientation(s).
@@ -1870,9 +2015,11 @@ class Mujoco(Simulator):
np.array[float[N,3]]: link frame position of each link in world space
np.array[float[N,4]]: orientation of each link frame [x,y,z,w]
"""
pass
pos = self._get_link_result(body_id, link_ids, self.sim.data.body_xpos)
quat = self._get_link_result(body_id, link_ids, self.sim.data.body_xquat, fct=self._convert_wxyz_to_xyzw)
return pos, quat
def get_link_world_positions(self, body_id, link_ids):
def get_link_world_positions(self, body_id, link_ids=None):
"""
Return the CoM position (in the Cartesian world space coordinates) of the given link(s).
@@ -1886,8 +2033,7 @@ class Mujoco(Simulator):
if multiple links:
np.array[float[N,3]]: CoM position of each link in world space
"""
# Cartesian position of body frame (same as xipos)
return np.asarray(self._get_link_result(body_id, link_ids, self.sim.data.body_xpos))
return self._get_link_result(body_id, link_ids, self.sim.data.xipos)
def get_link_positions(self, body_id, link_ids):
pass
@@ -1906,9 +2052,7 @@ class Mujoco(Simulator):
if multiple links:
np.array[float[N,4]]: CoM orientation of each link (x,y,z,w)
"""
# Cartesian orientation of body frame
return np.asarray(self._get_link_result(body_id, link_ids, self.sim.data.body_xquat,
fct=self._convert_wxyz_to_xyzw))
return self._get_link_result(body_id, link_ids, self.sim.data.body_xquat, fct=self._convert_wxyz_to_xyzw)
def get_link_orientations(self, body_id, link_ids):
pass
@@ -1927,7 +2071,7 @@ class Mujoco(Simulator):
if multiple links:
np.array[float[N,3]]: linear velocity of each link
"""
return np.asarray(self._get_link_result(body_id, link_ids, self.sim.data.cvel, slice=slice(3, 6)))
return self._get_link_result(body_id, link_ids, self.sim.data.cvel, slice=slice(3, 6))
def get_link_world_angular_velocities(self, body_id, link_ids):
"""
@@ -1961,9 +2105,7 @@ class Mujoco(Simulator):
np.array[float[N,6]]: linear and angular velocity of each link
"""
velocities = np.asarray(self._get_link_result(body_id, link_ids, self.sim.data.cvel))
if velocities.ndim == 1:
return np.roll(velocities, shift=3)
return np.roll(velocities, shift=3, axis=1)
return np.roll(velocities, shift=3, axis=velocities.ndim-1)
def get_link_velocities(self, body_id, link_ids):
pass
@@ -2018,9 +2160,7 @@ class Mujoco(Simulator):
np.array[float[N,6]]: linear and angular acceleration of each link
"""
accelerations = np.asarray(self._get_link_result(body_id, link_ids, self.sim.data.cacc))
if accelerations.ndim == 1:
return np.roll(accelerations, shift=3)
return np.roll(accelerations, shift=3, axis=1)
return np.roll(accelerations, shift=3, axis=accelerations.ndim - 1)
def get_q_indices(self, body_id, joint_ids):
"""
@@ -2094,7 +2234,17 @@ class Mujoco(Simulator):
int: joint type id.
if multiple joints: list of above
"""
pass
body = self._bodies[body_id]
one_joint = isinstance(joint_ids, int)
if one_joint:
joint_ids = [joint_ids]
types = []
for joint_id in joint_ids:
joint = body.get_joint(joint_id)
types.append(self._get_joint_type_id(joint.dtype))
if one_joint and len(types) > 1:
return types[0]
return types
def get_joint_type_names(self, body_id, joint_ids):
"""
@@ -2224,15 +2374,32 @@ 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.
"""
pass
# TODO: use the other arguments
def get_joint_positions(self, body_id, joint_ids):
body = self._bodies[body_id]
if joint_ids is None:
self.sim.data.qpos[body.q_idx1:body.q_idxf] = positions
# 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.qpos[body.q_idx1 + joint_ids] = positions
# if multiple joints, set their torques
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.qpos[body.q_idx1 + q[q != -1]] = positions
def get_joint_positions(self, body_id, joint_ids=None):
"""
Get the position of the given joint(s).
Args:
body_id (int): unique body id.
joint_ids (int, list[int]): joint id, or list of joint ids.
joint_ids (int, list[int], None): joint id, or list of joint ids. If None, it will take all the actuated
joints.
Returns:
if 1 joint:
@@ -2241,7 +2408,22 @@ class Mujoco(Simulator):
np.array[float[N]]: joint positions [rad]
"""
body = self._bodies[body_id]
pass
if joint_ids is None:
return self.sim.data.qpos[body.q_idx1:body.q_idxf]
# check if valid joint
self._check_joint_ids(body, joint_ids)
# if one joint, return its position
if isinstance(joint_ids, int):
return self.sim.data.qpos[body.q_idx1 + joint_ids]
# if multiple joints, return their positions
q = body.get_q_idx(joint_ids, keep=True) # E.g. [0, -1, 1, -1, 2, 3] (-1 are for fixed joints)
qpos = np.zeros(len(joint_ids))
qpos[q != -1] = self.sim.data.qpos[body.q_idx1 + q[q != -1]]
return qpos
def set_joint_velocities(self, body_id, joint_ids, velocities, max_force=None):
"""
@@ -2253,9 +2435,25 @@ 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
"""
pass
# TODO: use the other arguments
def get_joint_velocities(self, body_id, joint_ids):
body = self._bodies[body_id]
if joint_ids is None:
self.sim.data.qvel[body.v_idx1:body.v_idxf] = velocities
# 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.qvel[body.v_idx1 + joint_ids] = velocities
# if multiple joints, set their torques
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
def get_joint_velocities(self, body_id, joint_ids=None):
"""
Get the velocity of the given joint(s).
@@ -2269,7 +2467,23 @@ class Mujoco(Simulator):
if multiple joints:
np.array[float[N]]: joint velocities [rad/s]
"""
pass
body = self._bodies[body_id]
if joint_ids is None:
return self.sim.data.qvel[body.v_idx1:body.v_idxf]
# check if valid joint
self._check_joint_ids(body, joint_ids)
# if one joint, return its velocity
if isinstance(joint_ids, int):
return self.sim.data.qvel[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)
qvel = np.zeros(len(joint_ids))
qvel[q != -1] = self.sim.data.qvel[body.v_idx1 + q[q != -1]]
return qvel
def set_joint_accelerations(self, body_id, joint_ids, accelerations, q=None, dq=None):
"""
@@ -2280,11 +2494,11 @@ class Mujoco(Simulator):
body_id (int): unique body id.
joint_ids (int, list[int]): joint id, or list of joint ids.
accelerations (float, np.array[float[N]]): desired joint acceleration, or list of desired joint
accelerations [rad/s^2]
accelerations [rad/s^2]
"""
pass
def get_joint_accelerations(self, body_id, joint_ids): # , q=None, dq=None):
def get_joint_accelerations(self, body_id, joint_ids=None): # , q=None, dq=None):
"""
Get the acceleration of the specified joint(s). This is only valid if the simulator `supports_acceleration`.
@@ -2298,7 +2512,23 @@ class Mujoco(Simulator):
if multiple joints:
np.array[float[N]]: joint accelerations [rad/s^2]
"""
pass
body = self._bodies[body_id]
if joint_ids is None:
return self.sim.data.qacc[body.v_idx1:body.v_idxf]
# check if valid joint
self._check_joint_ids(body, joint_ids)
# if one joint, return its velocity
if isinstance(joint_ids, int):
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)
qacc = np.zeros(len(joint_ids))
qacc[q != -1] = self.sim.data.qacc[body.v_idx1 + q[q != -1]]
return qacc
def set_joint_torques(self, body_id, joint_ids, torques):
"""
@@ -2309,9 +2539,23 @@ 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].
"""
pass
body = self._bodies[body_id]
def get_joint_torques(self, body_id, joint_ids):
if joint_ids is None:
self.sim.data.qfrc_applied[body.v_idx1:body.v_idxf] = torques
# 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
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
def get_joint_torques(self, body_id, joint_ids=None):
"""
Get the applied torque(s) on the given joint(s).
@@ -2325,9 +2569,25 @@ class Mujoco(Simulator):
if multiple joints:
np.array[float[N]]: torques associated to the given joints [Nm]
"""
pass
body = self._bodies[body_id]
def get_joint_reaction_forces(self, body_id, joint_ids):
if joint_ids is None:
return self.sim.data.qfrc_applied[body.v_idx1:body.v_idxf]
# check if valid joint
self._check_joint_ids(body, joint_ids)
# if one joint, return its velocity
if isinstance(joint_ids, int):
return self.sim.data.qfrc_applied[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)
torques = np.zeros(len(joint_ids))
torques[q != -1] = self.sim.data.qfrc_applied[body.v_idx1 + q[q != -1]]
return torques
def get_joint_reaction_forces(self, body_id, joint_ids=None):
"""Return the joint reaction forces at the given joint. Note that the torque sensor must be enabled, otherwise
it will always return [0,0,0,0,0,0].
@@ -2341,7 +2601,32 @@ class Mujoco(Simulator):
if multiple joints:
np.array[float[N,6]]: joint reaction forces [N, Nm]
"""
pass
body = self._bodies[body_id]
if joint_ids is None:
# com-based interaction force with parent [torque, force]
force_parent = self.sim.data.cfrc_int[body.b_idx0:body.b_idxf]
# com-based external force on body [torque, force]
force_ext = self.sim.data.cfrc_ext[body.b_idx0:body.b_idxf]
force = force_ext - force_parent # TODO: is it + instead of -?
np.roll(force, shift=3, axis=force.ndim - 1) # [torque, force] --> [force, torque]
# TODO: express that force in the joint frame
return force
# check if valid link id
self._check_link_ids(body, joint_ids)
# com-based interaction force with parent [torque, force]
force_parent = self.sim.data.cfrc_int[body.b_idx0 + joint_ids]
# com-based external force on body [torque, force]
force_ext = self.sim.data.cfrc_ext[body.b_idx0 + joint_ids]
force = force_ext - force_parent # TODO: is it + instead of -?
np.roll(force, shift=3, axis=force.ndim-1) # [torque, force] --> [force, torque]
# TODO: express that force in the joint frame
return force
def get_joint_powers(self, body_id, joint_ids):
"""Return the applied power at the given joint(s). Power = torque * velocity.
@@ -2356,7 +2641,9 @@ class Mujoco(Simulator):
if multiple joints:
np.array[float[N]]: power at each joint [W]
"""
pass
torque = self.get_joint_torques(body_id, joint_ids)
velocity = self.get_joint_velocities(body_id, joint_ids)
return torque * velocity
#################
# Visualization #
@@ -2702,15 +2989,21 @@ class Mujoco(Simulator):
to_position (np.array[float[3]]): end of the ray in world coordinates
Returns:
list:
int: object unique id of the hit object
int: link index of the hit object, or -1 if none/parent
float: hit fraction along the ray in range [0,1] along the ray.
np.array[float[3]]: hit position in Cartesian world coordinates
np.array[float[3]]: hit normal in Cartesian world coordinates
[0] int: object unique id of the hit object
[1] int: link index of the hit object, or -1 if none/parent
[2] float: hit fraction along the ray in range [0,1] along the ray.
[3] np.array[float[3]]: hit position in Cartesian world coordinates
[4] np.array[float[3]]: hit normal in Cartesian world coordinates
"""
vec = to_position - from_position
return self.sim.ray(pnt=from_position, vec=vec) # this return the distance and id of the geom
distance, geom_id = self.sim.ray(pnt=from_position, vec=vec) # this return the distance and id of the geom
norm = np.linalg.norm(vec)
fraction = distance / norm
unit_vec = vec / norm
position = distance * unit_vec
normal = -unit_vec # TODO: this is not correct...
# TODO: get body_id and link_id from geom_id
return geom_id, geom_id, fraction, position, normal
###########################
# Kinematics and Dynamics #
@@ -2772,7 +3065,8 @@ class Mujoco(Simulator):
def change_dynamics(self, body_id, link_id=-1, mass=None, lateral_friction=None, spinning_friction=None,
rolling_friction=None, restitution=None, linear_damping=None, angular_damping=None,
contact_stiffness=None, contact_damping=None, friction_anchor=None,
local_inertia_diagonal=None, joint_damping=None):
local_inertia_diagonal=None, inertia_position=None, inertia_orientation=None,
joint_damping=None, joint_friction=None):
"""
Change dynamic properties of the given body (or link) such as mass, friction and restitution coefficients, etc.
@@ -2783,7 +3077,7 @@ class Mujoco(Simulator):
lateral_friction (float): lateral (linear) contact friction
spinning_friction (float): torsional friction around the contact normal
rolling_friction (float): torsional friction orthogonal to contact normal
restitution (float): bouncyness of contact. Keep it a bit less than 1.
restitution (float): bounciness of contact. Keep it a bit less than 1.
linear_damping (float): linear damping of the link (0.04 by default)
angular_damping (float): angular damping of the link (0.04 by default)
contact_stiffness (float): stiffness of the contact constraints, used together with `contact_damping`
@@ -2795,13 +3089,17 @@ class Mujoco(Simulator):
local_inertia_diagonal (np.array[float[3]]): diagonal elements of the inertia tensor. Note that the base
and links are centered around the center of mass and aligned with the principal axes of inertia so
there are no off-diagonal elements in the inertia tensor.
inertia_position (np.array[float[3]]): new inertia position with respect to the link frame.
inertia_orientation (np.array[float[4]]): new inertia orientation (expressed as a quaternion [x,y,z,w]
with respect to the link frame.
joint_damping (float): joint damping coefficient applied at each joint. This coefficient is read from URDF
joint damping field. Keep the value close to 0.
`joint_damping_force = -damping_coefficient * joint_velocity`.
joint_friction (float): joint friction coefficient.
"""
body = self._bodies[body_id]
if link_id < -1 or link_id > (body.num_bodies - 2): # -1 is for the base
raise ValueError("link_id should belong to [-1, `num_links-2`].")
self._check_link_id(body, link_id)
idx = body.b_idx0 + 1 + link_id
if mass is not None:
@@ -3092,6 +3390,53 @@ class Mujoco(Simulator):
qacc = dest.qacc[body.v_idx0:body.v_idxf]
return qacc
#########
# Debug #
#########
# TODO
############################
# Events (mouse, keyboard) #
############################
def get_keyboard_events(self):
"""Get the key events.
Returns:
dict: {keyId: keyState}
* `keyID` is an integer (ascii code) representing the key. Some special keys like shift, arrows,
and others are are defined in pybullet such as `B3G_SHIFT`, `B3G_LEFT_ARROW`, `B3G_UP_ARROW`,...
* `keyState` is an integer. 3 if the button has been pressed, 1 if the key is down, 2 if the key has
been triggered.
"""
pass
def get_mouse_events(self):
"""Get the mouse events.
Returns:
list of mouse events:
eventType (int): 1 if the mouse is moving, 2 if a button has been pressed or released
mousePosX (float): x-coordinates of the mouse pointer
mousePosY (float): y-coordinates of the mouse pointer
buttonIdx (int): button index for left/middle/right mouse button. It is -1 if nothing,
0 if left button, 1 if scroll wheel (pressed), 2 if right button
buttonState (int): 0 if nothing, 3 if the button has been pressed, 4 is the button has been released,
1 if the key is down (never observed), 2 if the key has been triggered (never
observed).
"""
pass
def get_mouse_and_keyboard_events(self):
"""Get the mouse and key events.
Returns:
list: list of mouse events
dict: dictionary of key events
"""
pass
# Test
if __name__ == '__main__':
pyrobolearn/simulators/simulator.py
+15 -9
View File
@@ -135,6 +135,7 @@ class Simulator(object):
JOINT_PRISMATIC = 1
JOINT_REVOLUTE = 0
JOINT_SPHERICAL = 2
JOINT_FREE = 7 # NEW
KEY_IS_DOWN = 1
KEY_WAS_RELEASED = 4
@@ -2361,7 +2362,8 @@ class Simulator(object):
def change_dynamics(self, body_id, link_id=-1, mass=None, lateral_friction=None, spinning_friction=None,
rolling_friction=None, restitution=None, linear_damping=None, angular_damping=None,
contact_stiffness=None, contact_damping=None, friction_anchor=None,
local_inertia_diagonal=None, joint_damping=None):
local_inertia_diagonal=None, inertia_position=None, inertia_orientation=None,
joint_damping=None, joint_friction=None):
"""
Change dynamic properties of the given body (or link) such as mass, friction and restitution coefficients, etc.
@@ -2372,21 +2374,25 @@ class Simulator(object):
lateral_friction (float): lateral (linear) contact friction
spinning_friction (float): torsional friction around the contact normal
rolling_friction (float): torsional friction orthogonal to contact normal
restitution (float): bouncyness of contact. Keep it a bit less than 1.
restitution (float): bounciness of contact. Keep it a bit less than 1.
linear_damping (float): linear damping of the link (0.04 by default)
angular_damping (float): angular damping of the link (0.04 by default)
contact_stiffness (float): stiffness of the contact constraints, used together with `contact_damping`
contact_damping (float): damping of the contact constraints for this body/link. Used together with
`contact_stiffness`. This overrides the value if it was specified in the URDF file in the contact
section.
`contact_stiffness`. This overrides the value if it was specified in the URDF file in the contact
section.
friction_anchor (int): enable or disable a friction anchor: positional friction correction (disabled by
default, unless set in the URDF contact section)
default, unless set in the URDF contact section)
local_inertia_diagonal (np.array[float[3]]): diagonal elements of the inertia tensor. Note that the base
and links are centered around the center of mass and aligned with the principal axes of inertia so
there are no off-diagonal elements in the inertia tensor.
and links are centered around the center of mass and aligned with the principal axes of inertia so
there are no off-diagonal elements in the inertia tensor.
inertia_position (np.array[float[3]]): new inertia position with respect to the link frame.
inertia_orientation (np.array[float[4]]): new inertia orientation (expressed as a quaternion [x,y,z,w]
with respect to the link frame.
joint_damping (float): joint damping coefficient applied at each joint. This coefficient is read from URDF
joint damping field. Keep the value close to 0.
`joint_damping_force = -damping_coefficient * joint_velocity`.
joint damping field. Keep the value close to 0.
`joint_damping_force = -damping_coefficient * joint_velocity`.
joint_friction (float): joint friction coefficient.
"""
pass
pyrobolearn/utils/parsers/robots/data_structures.py
+134 -11
View File
@@ -8,7 +8,7 @@ import trimesh
from collections import OrderedDict, Iterable
from pyrobolearn.utils.transformation import get_rpy_from_quaternion, get_quaternion_from_rpy, get_matrix_from_rpy, \
get_rpy_from_matrix, get_matrix_from_axis_angle
get_rpy_from_matrix, get_matrix_from_axis_angle, get_inverse_homogeneous
from pyrobolearn.utils.inertia import get_inertia_of_box, get_inertia_of_capsule, get_inertia_of_cylinder, \
get_inertia_of_ellipsoid, get_inertia_of_mesh, get_inertia_of_sphere, combine_inertias
@@ -142,7 +142,41 @@ class PhysicsEngine(object):
class Frame(object):
r"""Reference Frame"""
r"""Reference Frame.
This is used to expressed the position and orientation of frames that are used for bodies/links (inertials,
visuals, collisions) and joints.
Note that we follow the convention described in URDFs [1, 2] to describe the various frames. Notably,
- the link frame is the same as the joint frame and is at the base of a body/link.
- the inertial frame is expressed with respect to the link/joint frame.
- the visual frame is expressed with respect to the link/joint frame.
- the collision frame is expressed with respect to the link/joint frame.
- the child link/joint frame is expressed with respect to the parent link/joint frame.
This sometimes can conflict with other conventions such as the one followed in MuJoCo [3], where:
- In MuJoCo, the positions and orientations of all elements can be expressed in global or local coordinates in
the XML file. However, once compiled everything will be expressed in local coordinates. The local coordinates
are different from the ones defined in URDFs.
- the body/link frame is defined at the CoM of the body, thus at the inertial frame.
- the inertial, visual, and collision (geoms/sites) frames are expressed with respect to the body/link frame they
belong to.
- the child body frame is expressed with respect to the parent body frame.
- the joint frame that connects the parent and child body is expressed with respect to the child body frame.
Another one where there can be a conflict is with Bullet [4], where all the elements are expressed in local
coordinates and the convention is pretty similar to URDF, except:
- the link and joint frames are decoupled; the joint frame is the same as the link/joint frame in URDF but the
link frame is the same as Mujoco (i.e. it is at the inertial frame of the body).
- the next joint frame is expressed with respect to the inertial frame.
References:
- [1] http://wiki.ros.org/urdf/XML/link
- [2] http://wiki.ros.org/urdf/XML/joint
- [3] http://mujoco.org/book/modeling.html#CFrame
- [4] Pybullet Quickstart Guide:
https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit#heading=h.e27vav9dy7v6
"""
def __init__(self, position=None, orientation=None, dtype=None, right_handed=True):
"""
@@ -788,6 +822,34 @@ class MultiBody(object):
The multi-body / tree data structure starts with a root element (=base link) and contains each bodies / joints.
Each tree represents a multi-body in the world. Its position / orientation is expressed in the world frame.
Note that we follow the convention described in URDFs [1, 2] to describe the various frames. Notably,
- the link frame is the same as the joint frame and is at the base of a body/link.
- the inertial, visual, and collision frames are expressed with respect to the link/joint frame.
- the child link/joint frame is expressed with respect to the parent link/joint frame.
This sometimes can conflict with other conventions such as the one followed in MuJoCo [3], where:
- In MuJoCo, the positions and orientations of all elements can be expressed in global or local coordinates in
the XML file. However, once compiled everything will be expressed in local coordinates. The local coordinates
are different from the ones defined in URDFs.
- the body/link frame is defined at the CoM of the body, thus at the inertial frame.
- the inertial, visual, and collision (geoms/sites) frames are expressed with respect to the body/link frame they
belong to.
- the child body frame is expressed with respect to the parent body frame.
- the joint frame that connects the parent and child body is expressed with respect to the child body frame.
Another one where there can be a conflict is with Bullet [4], where all the elements are expressed in local
coordinates and the convention is pretty similar to URDF, except:
- the link and joint frames are decoupled; the joint frame is the same as the link/joint frame in URDF but the
link frame is the same as Mujoco (i.e. it is at the inertial frame of the body).
- the next joint frame is expressed with respect to the inertial frame.
References:
- [1] http://wiki.ros.org/urdf/XML/link
- [2] http://wiki.ros.org/urdf/XML/joint
- [3] http://mujoco.org/book/modeling.html#CFrame
- [4] Pybullet Quickstart Guide:
https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit#heading=h.e27vav9dy7v6
"""
def __init__(self, name=None, root=None, position=None, orientation=None):
@@ -801,9 +863,9 @@ class MultiBody(object):
orientation (list/tuple/np.array[float[3/4/9]], np.array[float[3,3]], str): frame orientation in the world.
"""
self.name = name
self.root = root
self.bodies = OrderedDict() # {name: Body}
self.joints = OrderedDict() # {name: Joint}
self.root = root
self.materials = {}
self.frame = Frame(position, orientation, dtype='world')
@@ -858,9 +920,12 @@ class MultiBody(object):
@root.setter
def root(self, root):
"""Set the root body element."""
if root is not None and not isinstance(root, Body):
raise TypeError("Expecting the given 'body' to be an instance of `Body`, but instead got: "
"{}".format(type(root)))
if root is not None:
if not isinstance(root, Body):
raise TypeError("Expecting the given 'body' to be an instance of `Body`, but instead got: "
"{}".format(type(root)))
if len(self.bodies) == 0: # only if it is empty
self.bodies[root.name] = root
self._root = root
@property
@@ -992,6 +1057,43 @@ class MultiBody(object):
Tree = MultiBody
def transform_inertial_frame_to_joint_frame(body):
"""Return the homogeneous transform from the inertial frame to the joint frame."""
# the inertial frame is expressed wrt to the joint frame by default
inertial = body.inertial
if inertial is not None:
return get_inverse_homogeneous(inertial.homogeneous)
def transform_child_joint_frame_to_parent_inertial_frame(child_body):
"""Return the homogeneous transform from the child joint frame to the parent inertial frame."""
parent_joint = child_body.parent_joint
parent = child_body.parent_body
if parent_joint is not None and parent.inertial is not None:
h_p_c = parent_joint.homogeneous # from parent to child link/joint frame
h_c_p = get_inverse_homogeneous(h_p_c) # from child to parent link/joint frame
h_p_pi = parent.inertial.homogeneous # from parent link/joint frame to inertial frame
h_c_pi = h_c_p.dot(h_p_pi) # from child link/joint frame to parent inertial frame
return h_c_pi
def transform_inertial_frame_to_child_link_frame(child_body):
"""Return the homogeneous transform from the parent inertial frame to the child link/joint frame."""
# from child link/joint frame to parent inertial frame
h_c_pi = transform_child_joint_frame_to_parent_inertial_frame(child_body)
if h_c_pi is not None:
return get_inverse_homogeneous(h_c_pi)
def transform_inertial_frame_to_child_inertial_frame(child_body):
"""Return the homogeneous transform from the parent inertial frame to the child inertial frame."""
if child_body.inertial is not None:
h_c_ci = child_body.inertial.homogeneous
h_pi_c = transform_inertial_frame_to_child_link_frame(child_body)
if h_pi_c is not None:
return h_pi_c.dot(h_c_ci) # from parent parent inertial frame to child inertial frame
class Body(object):
r"""Body / Link data structure."""
@@ -1022,7 +1124,7 @@ class Body(object):
self.name = name
self.joints = OrderedDict() # child joints
self.parent_joints = OrderedDict() # parent joints
self.parent_joints = OrderedDict() # parent joints: Warning each body should only have one parent joint!!
# set body properties
self.inertials = inertials
@@ -1264,6 +1366,25 @@ class Body(object):
"""Set the given homogeneous matrix."""
self.frame.homogeneous = matrix
@property
def parent_body(self):
"""Return the parent body."""
if self.parent_joints:
joint = self.parent_joints[next(iter(self.parent_joints))]
return joint.parent # this can be None (like for the root)
@property
def child_bodies(self):
"""Return the child bodies."""
if self.joints:
return [joint.child for joint in self.joints if joint.child is not None]
@property
def parent_joint(self):
"""Return the parent joint."""
if self.parent_joints:
return self.parent_joints[next(iter(self.parent_joints))]
def add_collision(self, collision):
"""
Add a collision shape to the list of collision shapes.
@@ -1349,14 +1470,14 @@ class Joint(object):
Joint types: fixed, floating/free, prismatic, revolute/hinge, continuous, gearbox, revolute2, ball, screw,
universal, and planar.
- fixed: no motions is allowed; both links are rigidly attached to each other.
- fixed/weld: no motions is allowed; both links are rigidly attached to each other.
- floating/free: allows motion for all 6 degrees of motion.
- prismatic: allows motion along 1 translational DoF.
- revolute/hinge: allows rotational motion around one axis (1 DoF).
- continuous: a revolute/hinge joint that doesn't have lower or upper limits.
- gearbox: geared revolute joint.
- gearbox/gear: geared revolute joint.
- revolute2: two revolute joints connected in series
- ball: a ball and socket joint which allows rotational motions around the 3 axis (3 DoFs).
- ball (=spherical): a ball and socket joint which allows rotational motions around the 3 axis (3 DoFs).
- screw: a single DoF joint wich coupled sliding and rotational motion
- universal: like a ball joint, but constrains one DoF
- planar: allows motion in a plane perpendicular to the axis.
@@ -1365,6 +1486,7 @@ class Joint(object):
- SDF: ball, fixed, gearbox, prismatic, revolute, revolute2, screw, universal
- Dart: ball, free (=floating), euler, prismatic, weld (=fixed), revolute, universal
- MuJoCo: ball, free (=floating), hinge (=revolute), slide (=prismatic)
- Bullet: fixed, gear, planar, point2point, prismatic, revolute, spherical (=ball)
By default, we follow the convention expressed in URDF to describe the frames. That is, the child joint frame is
described with respect to the parent joint/link frame.
@@ -1450,7 +1572,8 @@ class Joint(object):
elif dtype in {'free', 'floating'}:
dtype = 'floating'
self.num_dofs = 6
elif dtype == 'ball':
elif dtype in {'ball', 'spherical'}:
dtype = 'ball'
self.num_dofs = 3
elif dtype == 'continuous':
self.num_dofs = 1
pyrobolearn/utils/parsers/robots/mujoco_parser.py
+3
View File
@@ -1209,6 +1209,9 @@ class MuJoCoParser(WorldParser):
# the element for geoms, joints, sites, cameras and lights", and a joint defined in a body connects that body
# with its parent body.
# copy Tree just in case to not rewrite anything
tree = copy.deepcopy(tree)
h_bodies, h_joints, h_visuals, h_collisions, h_inertials = {}, {}, {}, {}, {}
for i, body in enumerate(tree.bodies.values()):
# print(body.name, body.homogeneous)
pyrobolearn/utils/parsers/robots/urdf_parser.py
+1 -1
View File
@@ -283,7 +283,7 @@ class URDFParser(RobotParser):
joint.friction = dynamics_tag.attrib.get('friction')
# limits
limits_tag = joint_tag.find('limits')
limits_tag = joint_tag.find('limit')
if limits_tag is not None:
joint.effort = limits_tag.attrib.get('effort')
joint.velocity = limits_tag.attrib.get('velocity')
pyrobolearn/worlds/world.py
+2 -1
View File
@@ -1195,7 +1195,8 @@ class World(object):
int: unique id of the floor in the world
"""
# self.floor_id = self.sim.load_urdf('plane100.urdf', use_fixed_base=True, scale=scaling)
self.floor_id = self.sim.load_urdf('plane.urdf', position=[0., 0., 0.], use_fixed_base=True, scale=scaling)
# self.floor_id = self.sim.load_urdf('plane.urdf', position=[0., 0., 0.], use_fixed_base=True, scale=scaling)
self.floor_id = self.sim.load_floor(dimension=scaling * 20)
# distance = self.camera.distance
# self.camera.reset(distance=scaling * distance)
return self.floor_id