def set_joint_position(self,
position: float,
disable_dynamics: bool = False) -> None:
"""Sets the intrinsic position of the joint.
:param disable_dynamics: If True, then the position can be set even
when the joint mode is in Force mode. It will disable dynamics,
move the joint, and then re-enable dynamics.
:param position: Position of a joint (angular or linear
values depending on the joint type).
"""
if not disable_dynamics:
sim.simSetJointPosition(self._handle, position)
return
is_model = self.is_model()
if not is_model:
self.set_model(True)
prior = sim.simGetModelProperty(self.get_handle())
p = prior | sim.sim_modelproperty_not_dynamic
# Disable the dynamics
sim.simSetModelProperty(self._handle, p)
with utils.step_lock:
sim.simExtStep(True) # Have to step for changes to take effect
sim.simSetJointPosition(self._handle, position)
self.set_joint_target_position(position)
with utils.step_lock:
sim.simExtStep(True) # Have to step for changes to take effect
# Re-enable the dynamics
sim.simSetModelProperty(self._handle, prior)
self.set_model(is_model)
def set_joint_position(self, position: float,
allow_force_mode=True) -> None:
"""Sets the intrinsic position of the joint.
:param positions: A list of positions of the joints (angular or linear
values depending on the joint type).
:param allow_force_mode: If True, then the position can be set even
when the joint mode is in Force mode. It will disable dynamics,
move the joint, and then re-enable dynamics.
"""
if not allow_force_mode:
sim.simSetJointPosition(self._handle, position)
return
is_model = self.is_model()
if not is_model:
self.set_model(True)
prior = sim.simGetModelProperty(self.get_handle())
p = prior | sim.sim_modelproperty_not_dynamic
# Disable the dynamics
sim.simSetModelProperty(self._handle, p)
sim.simSetJointPosition(self._handle, position)
self.set_joint_target_position(position)
sim.simExtStep(True) # Have to step once for changes to take effect
# Re-enable the dynamics
sim.simSetModelProperty(self._handle, prior)
self.set_model(is_model)
def is_model(self) -> bool:
"""Whether the object is a model or not.
:return: If the object is a model.
"""
prop = sim.simGetModelProperty(self._handle)
return not (prop & sim.sim_modelproperty_not_model)
def set_model(self, value: bool):
"""Set whether the object is a model or not.
:param value: True to set as a model.
"""
current = sim.simGetModelProperty(self._handle)
current |= sim.sim_modelproperty_not_model
if value:
current -= sim.sim_modelproperty_not_model
sim.simSetModelProperty(self._handle, current)
def set_joint_positions(self,
positions: List[float],
disable_dynamics: bool = False) -> None:
"""Sets the intrinsic position of the joints.
See :py:meth:`Joint.set_joint_position` for more information.
:param disable_dynamics: If True, then the position can be set even
when the joint mode is in Force mode. It will disable dynamics,
move the joint, and then re-enable dynamics.
:param positions: A list of positions of the joints (angular or linear
values depending on the joint type).
"""
self._assert_len(positions)
if not disable_dynamics:
[
sim.simSetJointPosition(jh, p) # type: ignore
for jh, p in zip(self._joint_handles, positions)
]
return
is_model = self.is_model()
if not is_model:
self.set_model(True)
prior = sim.simGetModelProperty(self.get_handle())
p = prior | sim.sim_modelproperty_not_dynamic
# Disable the dynamics
sim.simSetModelProperty(self._handle, p)
with utils.step_lock:
sim.simExtStep(True) # Have to step for changes to take effect
[
sim.simSetJointPosition(jh, p) # type: ignore
for jh, p in zip(self._joint_handles, positions)
]
[
j.set_joint_target_position(p) # type: ignore
for j, p in zip(self.joints, positions)
]
with utils.step_lock:
sim.simExtStep(True) # Have to step for changes to take effect
# Re-enable the dynamics
sim.simSetModelProperty(self._handle, prior)
self.set_model(is_model)
def _set_model_property(self, prop_type: int, value: bool) -> None:
current = sim.simGetModelProperty(self._handle)
current |= prop_type # Makes is not X
if value:
current -= prop_type
sim.simSetModelProperty(self._handle, current)
def _get_model_property(self, prop_type: int) -> bool:
current = sim.simGetModelProperty(self._handle)
return (current & prop_type) == 0
