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:
vrep.simSetJointPosition(self._handle, position)
return
is_model = self.is_model()
if not is_model:
self.set_model(True)
prior = vrep.simGetModelProperty(self.get_handle())
p = prior | vrep.sim_modelproperty_not_dynamic
# Disable the dynamics
vrep.simSetModelProperty(self._handle, p)
vrep.simSetJointPosition(self._handle, position)
self.set_joint_target_position(position)
vrep.simExtStep(True) # Have to step once for changes to take effect
# Re-enable the dynamics
vrep.simSetModelProperty(self._handle, prior)
self.set_model(is_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 = vrep.simGetModelProperty(self._handle)
current |= vrep.sim_modelproperty_not_model
if value:
current -= vrep.sim_modelproperty_not_model
vrep.simSetModelProperty(self._handle, current)
def _set_joint_position_torque_force_mode(self, value: float) -> None:
p = vrep.simGetModelProperty(self.get_handle())
p |= vrep.sim_modelproperty_not_dynamic
# Disable the dynamics
vrep.simSetModelProperty(self._handle, p)
vrep.simSetConfigurationTree(self._init_config_tree)
# Set the joint angles
vrep.simSetJointPosition(self._handle, value)
vrep.simSetJointTargetPosition(self._handle, value)
p = vrep.simGetModelProperty(self.get_handle())
p = ((p | vrep.sim_modelproperty_not_dynamic) -
vrep.sim_modelproperty_not_dynamic)
# Re-enable the dynamics
vrep.simSetModelProperty(self._handle, p)
def set_joint_positions(self,
positions: List[float],
allow_force_mode=True) -> None:
"""Sets the intrinsic position of the joints.
See :py:meth:`Joint.set_joint_position` for more information.
: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.
"""
self._assert_len(positions)
if not allow_force_mode:
[
j.set_joint_position(p, allow_force_mode)
for j, p in zip(self.joints, positions)
]
return
is_model = self.is_model()
if not is_model:
self.set_model(True)
prior = vrep.simGetModelProperty(self.get_handle())
p = prior | vrep.sim_modelproperty_not_dynamic
# Disable the dynamics
vrep.simSetModelProperty(self._handle, p)
[
j.set_joint_position(p, allow_force_mode)
for j, p in zip(self.joints, positions)
]
[
j.set_joint_target_position(p)
for j, p in zip(self.joints, positions)
]
vrep.simExtStep(True) # Have to step once for changes to take effect
# Re-enable the dynamics
vrep.simSetModelProperty(self._handle, prior)
self.set_model(is_model)
def _set_model_property(self, prop_type: int, value: bool) -> None:
current = vrep.simGetModelProperty(self._handle)
current |= prop_type # Makes is not X
if value:
current -= prop_type
vrep.simSetModelProperty(self._handle, current)