def set_velocity(self, lf, rf, lb, rb) -> None:
v = [lf, rf, lb, rb]
# sim.simSetJointForce(self._handle,100)
for i in range(2):
sim.simSetJointForce(self.motorHandles[2 * i], self.joint_force)
sim.simSetJointTargetVelocity(self.motorHandles[2 * i], v[2 * i])
for i in range(2):
sim.simSetJointForce(self.motorHandles[2 * i + 1],
self.joint_force)
sim.simSetJointTargetVelocity(self.motorHandles[2 * i + 1],
-v[2 * i + 1])
def set_joint_force(self, force: float) -> None:
"""Sets the maximum force or torque that a joint can exert.
The joint will apply that force/torque until the joint target velocity
has been reached. To apply a negative force/torque, set a negative
target velocity. This function has no effect when the joint is not
dynamically enabled, or when it is a spherical joint.
:param force: The maximum force or torque that the joint can exert.
This cannot be a negative value.
"""
sim.simSetJointForce(self._handle, force)
def set_torque(self, lf, rf, lb, rb) -> None:
t = [lf, rf, lb, rb]
for i in range(2):
if t[2 * i] > 0:
sim.simSetJointForce(self.motorHandles[2 * i], t[2 * i])
sim.simSetJointTargetVelocity(
self.motorHandles[2 * i],
1000000000000000) #unreachable value
else:
sim.simSetJointForce(self.motorHandles[2 * i], -t[2 * i])
sim.simSetJointTargetVelocity(
self.motorHandles[2 * i],
-1000000000000000) #unreachable value
for i in range(2):
if t[2 * i + 1] > 0:
sim.simSetJointForce(self.motorHandles[2 * i + 1],
t[2 * i + 1])
sim.simSetJointTargetVelocity(
self.motorHandles[2 * i + 1],
-1000000000000000) #unreachable value
else:
sim.simSetJointForce(self.motorHandles[2 * i + 1],
-t[2 * i + 1])
sim.simSetJointTargetVelocity(
self.motorHandles[2 * i + 1],
1000000000000000) #unreachable value
def __init__(self, name, z_pos=0.3089):
self._handle = sim.simGetObjectHandle(name)
self.motorHandles = []
self.motorHandles.append(
sim.simGetObjectHandle('joint_front_left_wheel'))
self.motorHandles.append(
sim.simGetObjectHandle('joint_front_right_wheel'))
self.motorHandles.append(
sim.simGetObjectHandle('joint_back_left_wheel'))
self.motorHandles.append(
sim.simGetObjectHandle('joint_back_right_wheel'))
self.joint_force = 10
sim.simSetJointForce(self.motorHandles[0], self.joint_force)
sim.simSetJointForce(self.motorHandles[1], self.joint_force)
sim.simSetJointForce(self.motorHandles[2], self.joint_force)
sim.simSetJointForce(self.motorHandles[3], self.joint_force)
# self.robot_collection_handle = lib.simCreateCollection(b'nikbot',b'1')
# lib.simAddObjectToCollection(self.robot_collection_handle,self._handle,b'1',b'00')
# self.robot_collection_handle = sim.simGetCollectionHandle('nikbot')
self.z_pos = z_pos
self.wheel_distance = 0.38578
self.wheel_radius = 0.23485