Python pybullet.setJointMotorControl2() Examples
The following are 30
code examples of pybullet.setJointMotorControl2().
You can vote up the ones you like or vote down the ones you don't like,
and go to the original project or source file by following the links above each example.
You may also want to check out all available functions/classes of the module
pybullet
, or try the search function
.
.
Example #1
| Source File: panda_env.py From pybullet-robot-envs with GNU Lesser General Public License v2.1 | 6 votes |
|
def apply_action_fingers(self, action, obj_id=None):
# move finger joints in position control
assert len(action) == 2, ('finger joints are 2! The number of actions you passed is ', len(action))
idx_fingers = [self._joint_name_to_ids['panda_finger_joint1'], self._joint_name_to_ids['panda_finger_joint2']]
# use object id to check contact force and eventually stop the finger motion
if obj_id is not None:
_, forces = self.check_contact_fingertips(obj_id)
# print("contact forces {}".format(forces))
if forces[0] >= 20.0:
action[0] = p.getJointState(self.robot_id, idx_fingers[0], physicsClientId=self._physics_client_id)[0]
if forces[1] >= 20.0:
action[1] = p.getJointState(self.robot_id, idx_fingers[1], physicsClientId=self._physics_client_id)[0]
for i, idx in enumerate(idx_fingers):
p.setJointMotorControl2(self.robot_id,
idx,
p.POSITION_CONTROL,
targetPosition=action[i],
force=10,
maxVelocity=1,
physicsClientId=self._physics_client_id)
Example #2
| Source File: cartpole_bullet.py From soccer-matlab with BSD 2-Clause "Simplified" License | 6 votes |
|
def _step(self, action):
p.stepSimulation()
# time.sleep(self.timeStep)
self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
theta, theta_dot, x, x_dot = self.state
dv = 0.1
deltav = [-10.*dv,-5.*dv, -2.*dv, -0.1*dv, 0, 0.1*dv, 2.*dv,5.*dv, 10.*dv][action]
p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, targetVelocity=(deltav + self.state[3]))
done = x < -self.x_threshold \
or x > self.x_threshold \
or theta < -self.theta_threshold_radians \
or theta > self.theta_threshold_radians
reward = 1.0
return np.array(self.state), reward, done, {}
Example #3
| Source File: motorController.py From Boston_Dynamics_Atlas_Explained with MIT License | 6 votes |
|
def setMotorsAngleInRealTimestep(self, motorTargetAngles, motorTargetTime, delayTime):
if(motorTargetTime == 0):
self._joint_targetPos = np.array(motorTargetAngles)
for i in range(self._joint_number):
p.setJointMotorControl2(bodyIndex=self._robot, jointIndex=self._joint_id[i], controlMode=p.POSITION_CONTROL,
targetPosition=self._joint_targetPos[i],
positionGain=self._kp, velocityGain=self._kd, force=self._torque, maxVelocity=self._max_velocity, physicsClientId=self._physicsClientId)
time.sleep(delayTime)
else:
self._joint_currentPos = self._joint_targetPos
self._joint_targetPos = np.array(motorTargetAngles)
for i in range(self._joint_number):
dydt = (self._joint_targetPos-self._joint_currentPos)/motorTargetTime
internalTime = 0.0
reft = time.time()
while internalTime < motorTargetTime:
internalTime = time.time() - reft
for i in range(self._joint_number):
p.setJointMotorControl2(bodyIndex=self._robot, jointIndex=self._joint_id[i], controlMode=p.POSITION_CONTROL,
targetPosition=self._joint_currentPos[i] + dydt[i] * internalTime,
positionGain=self._kp, velocityGain=self._kd, force=self._torque, maxVelocity=self._max_velocity, physicsClientId=self._physicsClientId)
Example #4
| Source File: bullet_physics_engine.py From NTP-vat-release with MIT License | 6 votes |
|
def control_joint_pos(body, joint, pos, max_vel=None, max_force=None):
if max_vel is None:
p.setJointMotorControl2(
bodyIndex=body,
jointIndex=joint,
controlMode=p.POSITION_CONTROL,
targetPosition=pos,
force=max_force)
else:
p.setJointMotorControl2(
bodyIndex=body,
jointIndex=joint,
controlMode=p.POSITION_CONTROL,
targetPosition=pos,
targetVelocity=max_vel,
force=max_force)
Example #5
| Source File: iiwa_robotiq_3_finger.py From costar_plan with Apache License 2.0 | 6 votes |
|
def gripper(self, cmd, mode=pb.POSITION_CONTROL):
'''
Gripper commands need to be mirrored to simulate behavior of the actual
UR5.
'''
pb.setJointMotorControl2(self.handle, self.left_knuckle, mode, -cmd)
pb.setJointMotorControl2(
self.handle, self.left_inner_knuckle, mode, -cmd)
pb.setJointMotorControl2(self.handle, self.left_finger, mode, cmd)
pb.setJointMotorControl2(self.handle, self.left_fingertip, mode, cmd)
pb.setJointMotorControl2(self.handle, self.right_knuckle, mode, -cmd)
pb.setJointMotorControl2(
self.handle, self.right_inner_knuckle, mode, -cmd)
pb.setJointMotorControl2(self.handle, self.right_finger, mode, cmd)
pb.setJointMotorControl2(self.handle, self.right_fingertip, mode, cmd)
Example #6
| Source File: minitaur.py From midlevel-reps with MIT License | 5 votes |
|
def _SetMotorTorqueById(self, motor_id, torque):
p.setJointMotorControl2(bodyIndex=self.minitaur,
jointIndex=motor_id,
controlMode=p.TORQUE_CONTROL,
force=torque)
Example #7
| Source File: robot_bases.py From GtS with MIT License | 5 votes |
|
def disable_motor(self):
p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,controlMode=p.POSITION_CONTROL, targetPosition=0, targetVelocity=0, positionGain=0.1, velocityGain=0.1, force=0)
Example #8
| Source File: minitaur.py From midlevel-reps with MIT License | 5 votes |
|
def _SetDesiredMotorAngleById(self, motor_id, desired_angle):
p.setJointMotorControl2(bodyIndex=self.minitaur,
jointIndex=motor_id,
controlMode=p.POSITION_CONTROL,
targetPosition=desired_angle,
positionGain=self._kp,
velocityGain=self._kd,
force=self.max_force)
Example #9
| Source File: robot_bases.py From midlevel-reps with MIT License | 5 votes |
|
def set_position(self, position):
"""Set position of joint
Position is defined in real world scale """
if self.jointType == p.JOINT_PRISMATIC:
position = np.array(position) / self.scale
p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,p.POSITION_CONTROL, targetPosition=position)
Example #10
| Source File: robot_bases.py From midlevel-reps with MIT License | 5 votes |
|
def set_velocity(self, velocity):
"""Set velocity of joint
Velocity is defined in real world scale """
if self.jointType == p.JOINT_PRISMATIC:
velocity = np.array(velocity) / self.scale
p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,p.VELOCITY_CONTROL, targetVelocity=velocity) # , positionGain=0.1, velocityGain=0.1)
Example #11
| Source File: robot_bases.py From midlevel-reps with MIT License | 5 votes |
|
def disable_motor(self):
p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,controlMode=p.POSITION_CONTROL, targetPosition=0, targetVelocity=0, positionGain=0.1, velocityGain=0.1, force=0)
Example #12
| Source File: gym_mujoco_xml_env.py From bullet-gym with MIT License | 5 votes |
|
def set_position(self, position): p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,p.POSITION_CONTROL, targetPosition=position)
Example #13
| Source File: gym_mujoco_xml_env.py From bullet-gym with MIT License | 5 votes |
|
def set_velocity(self, velocity): p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,p.VELOCITY_CONTROL, targetVelocity=velocity)
Example #14
| Source File: gym_mujoco_xml_env.py From bullet-gym with MIT License | 5 votes |
|
def set_torque(self, torque): p.setJointMotorControl2(bodyIndex=self.bodies[self.bodyIndex], jointIndex=self.jointIndex, controlMode=p.TORQUE_CONTROL, force=torque) #, positionGain=0.1, velocityGain=0.1)
Example #15
| Source File: gym_mujoco_xml_env.py From bullet-gym with MIT License | 5 votes |
|
def disable_motor(self): p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,controlMode=p.VELOCITY_CONTROL, force=0)
Example #16
| Source File: MJCFCommon.py From bullet-gym with MIT License | 5 votes |
|
def set_position(self, position):
p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,p.POSITION_CONTROL, targetPosition=position)
Example #17
| Source File: MJCFCommon.py From bullet-gym with MIT License | 5 votes |
|
def set_torque(self, torque):
p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,p.TORQUE_CONTROL, force=torque)
Example #18
| Source File: MJCFCommon.py From bullet-gym with MIT License | 5 votes |
|
def disable_motor(self):
p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,controlMode=p.VELOCITY_CONTROL, force=0)
Example #19
| Source File: motorController.py From bipedal-robot-walking-simulation with MIT License | 5 votes |
|
def setMotorAngle(self, motorTargetAngles):
for i in range(self._joint_number):
self._joint_targetPos[i] = motorTargetAngles[i]
p.setJointMotorControl2(
bodyIndex=self._robot,
jointIndex=self._joint_id[i],
controlMode=p.POSITION_CONTROL,
targetPosition=self._joint_targetPos[i],
positionGain=self._kp,
velocityGain=self._kd,
force=self._torque,
maxVelocity=self._max_velocity,
physicsClientId=self._physicsClientId)
Example #20
| Source File: motorController.py From bipedal-robot-walking-simulation with MIT License | 5 votes |
|
def setMotorsAngleInRealTimestep(self, motorTargetAngles, motorTargetTime, delayTime):
if (motorTargetTime == 0):
self._joint_targetPos = np.array(motorTargetAngles)
for i in range(self._joint_number):
p.setJointMotorControl2(
bodyIndex=self._robot,
jointIndex=self._joint_id[i],
controlMode=p.POSITION_CONTROL,
targetPosition=self._joint_targetPos[i],
positionGain=self._kp,
velocityGain=self._kd,
force=self._torque,
maxVelocity=self._max_velocity,
physicsClientId=self._physicsClientId)
time.sleep(delayTime)
else:
self._joint_currentPos = self._joint_targetPos
self._joint_targetPos = np.array(motorTargetAngles)
for i in range(self._joint_number):
dydt = (self._joint_targetPos - self._joint_currentPos) / motorTargetTime
internalTime = 0.0
reft = time.time()
while internalTime < motorTargetTime:
internalTime = time.time() - reft
for i in range(self._joint_number):
p.setJointMotorControl2(
bodyIndex=self._robot,
jointIndex=self._joint_id[i],
controlMode=p.POSITION_CONTROL,
targetPosition=self._joint_currentPos[i] + dydt[i] * internalTime,
positionGain=self._kp,
velocityGain=self._kd,
force=self._torque,
maxVelocity=self._max_velocity,
physicsClientId=self._physicsClientId)
Example #21
| Source File: motorController.py From bipedal-robot-walking-simulation with MIT License | 5 votes |
|
def setMotorsAngleInFixedTimestep(self, motorTargetAngles, motorTargetTime, delayTime):
if (motorTargetTime == 0):
self._joint_targetPos = np.array(motorTargetAngles)
for i in range(self._joint_number):
p.setJointMotorControl2(
bodyIndex=self._robot,
jointIndex=self._joint_id[i],
controlMode=p.POSITION_CONTROL,
targetPosition=self._joint_targetPos[i],
positionGain=self._kp,
velocityGain=self._kd,
force=self._torque,
maxVelocity=self._max_velocity,
physicsClientId=self._physicsClientId)
p.stepSimulation(physicsClientId=self._physicsClientId)
else:
self._joint_currentPos = self._joint_targetPos
self._joint_targetPos = np.array(motorTargetAngles)
for i in range(self._joint_number):
dydt = (self._joint_targetPos - self._joint_currentPos) / motorTargetTime
internalTime = 0.0
while internalTime < motorTargetTime:
internalTime += self._timeStep
for i in range(self._joint_number):
p.setJointMotorControl2(
bodyIndex=self._robot,
jointIndex=self._joint_id[i],
controlMode=p.POSITION_CONTROL,
targetPosition=self._joint_currentPos[i] + dydt[i] * internalTime,
positionGain=self._kp,
velocityGain=self._kd,
force=self._torque,
maxVelocity=self._max_velocity,
physicsClientId=self._physicsClientId)
p.stepSimulation(physicsClientId=self._physicsClientId)
if delayTime != 0:
for _ in range(int(delayTime / self._timeStep)):
p.stepSimulation(physicsClientId=self._physicsClientId)
Example #22
| Source File: motorController.py From Boston_Dynamics_Atlas_Explained with MIT License | 5 votes |
|
def setMotorAngle(self, motorTargetAngles):
for i in range(self._joint_number):
self._joint_targetPos[i] = motorTargetAngles[i]
p.setJointMotorControl2(bodyIndex=self._robot, jointIndex=self._joint_id[i], controlMode=p.POSITION_CONTROL,
targetPosition=self._joint_targetPos[i],
positionGain=self._kp, velocityGain=self._kd, force=self._torque, maxVelocity=self._max_velocity, physicsClientId=self._physicsClientId)
Example #23
| Source File: motorController.py From Boston_Dynamics_Atlas_Explained with MIT License | 5 votes |
|
def setMotorsAngleInFixedTimestep(self, motorTargetAngles, motorTargetTime, delayTime):
if(motorTargetTime == 0):
self._joint_targetPos = np.array(motorTargetAngles)
for i in range(self._joint_number):
p.setJointMotorControl2(bodyIndex=self._robot, jointIndex=self._joint_id[i], controlMode=p.POSITION_CONTROL,
targetPosition=self._joint_targetPos[i],
positionGain=self._kp, velocityGain=self._kd, force=self._torque, maxVelocity=self._max_velocity, physicsClientId=self._physicsClientId)
p.stepSimulation(physicsClientId=self._physicsClientId)
time.sleep(self._timeStep, physicsClientId=self._physicsClientId)
else:
self._joint_currentPos = self._joint_targetPos
self._joint_targetPos = np.array(motorTargetAngles)
for i in range(self._joint_number):
dydt = (self._joint_targetPos-self._joint_currentPos)/motorTargetTime
internalTime = 0.0
while internalTime < motorTargetTime:
internalTime += self._timeStep
for i in range(self._joint_number):
p.setJointMotorControl2(bodyIndex=self._robot, jointIndex=self._joint_id[i], controlMode=p.POSITION_CONTROL,
targetPosition=self._joint_currentPos[i] + dydt[i] * internalTime,
positionGain=self._kp, velocityGain=self._kd, force=self._torque, maxVelocity=self._max_velocity, physicsClientId=self._physicsClientId)
p.stepSimulation(physicsClientId=self._physicsClientId)
if delayTime != 0:
for _ in range(int(delayTime/self._timeStep)):
p.stepSimulation(physicsClientId=self._physicsClientId)
Example #24
| Source File: balancebot_env.py From balance-bot with MIT License | 5 votes |
|
def _assign_throttle(self, action):
dv = 0.1
deltav = [-10.*dv,-5.*dv, -2.*dv, -0.1*dv, 0, 0.1*dv, 2.*dv,5.*dv, 10.*dv][action]
vt = clamp(self.vt + deltav, -self.maxV, self.maxV)
self.vt = vt
p.setJointMotorControl2(bodyUniqueId=self.botId,
jointIndex=0,
controlMode=p.VELOCITY_CONTROL,
targetVelocity=vt)
p.setJointMotorControl2(bodyUniqueId=self.botId,
jointIndex=1,
controlMode=p.VELOCITY_CONTROL,
targetVelocity=-vt)
Example #25
| Source File: panda_env.py From pybullet-robot-envs with GNU Lesser General Public License v2.1 | 5 votes |
|
def reset(self):
# Load robot model
flags = p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES | p.URDF_USE_INERTIA_FROM_FILE
self.robot_id = p.loadURDF(os.path.join(franka_panda.get_data_path(), "panda_model.urdf"),
basePosition=self._base_position, useFixedBase=True, flags=flags,
physicsClientId=self._physics_client_id)
assert self.robot_id is not None, "Failed to load the panda model"
# reset joints to home position
num_joints = p.getNumJoints(self.robot_id, physicsClientId=self._physics_client_id)
idx = 0
for i in range(num_joints):
joint_info = p.getJointInfo(self.robot_id, i, physicsClientId=self._physics_client_id)
joint_name = joint_info[1].decode("UTF-8")
joint_type = joint_info[2]
if joint_type is p.JOINT_REVOLUTE or joint_type is p.JOINT_PRISMATIC:
assert joint_name in self.initial_positions.keys()
self._joint_name_to_ids[joint_name] = i
p.resetJointState(self.robot_id, i, self.initial_positions[joint_name], physicsClientId=self._physics_client_id)
p.setJointMotorControl2(self.robot_id, i, p.POSITION_CONTROL,
targetPosition=self.initial_positions[joint_name],
positionGain=0.2, velocityGain=1.0,
physicsClientId=self._physics_client_id)
idx += 1
self.ll, self.ul, self.jr, self.rs = self.get_joint_ranges()
if self._use_IK:
self._home_hand_pose = [0.2, 0.0, 0.8,
min(m.pi, max(-m.pi, m.pi)),
min(m.pi, max(-m.pi, 0)),
min(m.pi, max(-m.pi, 0))]
self.apply_action(self._home_hand_pose)
p.stepSimulation(physicsClientId=self._physics_client_id)
Example #26
| Source File: world_creation.py From assistive-gym with MIT License | 5 votes |
|
def setup_human_joints(self, human, joints_positions, controllable_joints, use_static_joints=True, human_reactive_force=None, human_reactive_gain=0.05):
if self.human_impairment != 'tremor':
self.human_tremors = np.zeros(len(controllable_joints))
elif len(controllable_joints) == 4:
self.human_tremors = self.np_random.uniform(np.deg2rad(-20), np.deg2rad(20), size=len(controllable_joints))
else:
self.human_tremors = self.np_random.uniform(np.deg2rad(-10), np.deg2rad(10), size=len(controllable_joints))
# Set starting joint positions
human_joint_states = p.getJointStates(human, jointIndices=list(range(p.getNumJoints(human, physicsClientId=self.id))), physicsClientId=self.id)
human_joint_positions = np.array([x[0] for x in human_joint_states])
for j in range(p.getNumJoints(human, physicsClientId=self.id)):
set_position = None
for j_index, j_angle in joints_positions:
if j == j_index:
p.resetJointState(human, jointIndex=j, targetValue=j_angle, targetVelocity=0, physicsClientId=self.id)
set_position = j_angle
break
if use_static_joints and j not in controllable_joints:
# Make all non controllable joints on the person static by setting mass of each link (joint) to 0
p.changeDynamics(human, j, mass=0, physicsClientId=self.id)
# Set velocities to 0
p.resetJointState(human, jointIndex=j, targetValue=human_joint_positions[j] if set_position is None else set_position, targetVelocity=0, physicsClientId=self.id)
# By default, all joints have motors enabled by default that prevent free motion. Disable these motors in human
for j in range(p.getNumJoints(human, physicsClientId=self.id)):
p.setJointMotorControl2(human, jointIndex=j, controlMode=p.VELOCITY_CONTROL, force=0, physicsClientId=self.id)
self.enforce_joint_limits(human)
if human_reactive_force is not None:
# NOTE: This runs a Position / Velocity PD controller for each joint motor on the human
human_joint_states = p.getJointStates(human, jointIndices=controllable_joints, physicsClientId=self.id)
target_human_joint_positions = np.array([x[0] for x in human_joint_states])
forces = [human_reactive_force * self.human_strength] * len(target_human_joint_positions)
p.setJointMotorControlArray(human, jointIndices=controllable_joints, controlMode=p.POSITION_CONTROL, targetPositions=target_human_joint_positions, positionGains=np.array([human_reactive_gain]*len(forces)), forces=forces, physicsClientId=self.id)
Example #27
| Source File: minitaur.py From soccer-matlab with BSD 2-Clause "Simplified" License | 5 votes |
|
def setMotorAngleById(self, motorId, desiredAngle):
p.setJointMotorControl2(bodyIndex=self.quadruped, jointIndex=motorId, controlMode=p.POSITION_CONTROL, targetPosition=desiredAngle, positionGain=self.kp, velocityGain=self.kd, force=self.maxForce)
Example #28
| Source File: saveRestoreStateTest.py From soccer-matlab with BSD 2-Clause "Simplified" License | 5 votes |
|
def setupWorld(self):
numObjects = 50
maximalCoordinates = False
p.resetSimulation()
p.setPhysicsEngineParameter(deterministicOverlappingPairs=1)
p.loadURDF("planeMesh.urdf",useMaximalCoordinates=maximalCoordinates)
kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf",[0,0,10], useMaximalCoordinates=maximalCoordinates)
for i in range (p.getNumJoints(kukaId)):
p.setJointMotorControl2(kukaId,i,p.POSITION_CONTROL,force=0)
for i in range (numObjects):
cube = p.loadURDF("cube_small.urdf",[0,i*0.02,(i+1)*0.2])
#p.changeDynamics(cube,-1,mass=100)
p.stepSimulation()
p.setGravity(0,0,-10)
Example #29
| Source File: kuka.py From soccer-matlab with BSD 2-Clause "Simplified" License | 5 votes |
|
def reset(self):
objects = p.loadSDF(os.path.join(self.urdfRootPath,"kuka_iiwa/kuka_with_gripper2.sdf"))
self.kukaUid = objects[0]
#for i in range (p.getNumJoints(self.kukaUid)):
# print(p.getJointInfo(self.kukaUid,i))
p.resetBasePositionAndOrientation(self.kukaUid,[-0.100000,0.000000,0.070000],[0.000000,0.000000,0.000000,1.000000])
self.jointPositions=[ 0.006418, 0.413184, -0.011401, -1.589317, 0.005379, 1.137684, -0.006539, 0.000048, -0.299912, 0.000000, -0.000043, 0.299960, 0.000000, -0.000200 ]
self.numJoints = p.getNumJoints(self.kukaUid)
for jointIndex in range (self.numJoints):
p.resetJointState(self.kukaUid,jointIndex,self.jointPositions[jointIndex])
p.setJointMotorControl2(self.kukaUid,jointIndex,p.POSITION_CONTROL,targetPosition=self.jointPositions[jointIndex],force=self.maxForce)
self.trayUid = p.loadURDF(os.path.join(self.urdfRootPath,"tray/tray.urdf"), 0.640000,0.075000,-0.190000,0.000000,0.000000,1.000000,0.000000)
self.endEffectorPos = [0.537,0.0,0.5]
self.endEffectorAngle = 0
self.motorNames = []
self.motorIndices = []
for i in range (self.numJoints):
jointInfo = p.getJointInfo(self.kukaUid,i)
qIndex = jointInfo[3]
if qIndex > -1:
#print("motorname")
#print(jointInfo[1])
self.motorNames.append(str(jointInfo[1]))
self.motorIndices.append(i)
Example #30
| Source File: baxter_ik_controller.py From robosuite with MIT License | 5 votes |
|
def sync_ik_robot(self, joint_positions, simulate=False, sync_last=True):
"""
Force the internal robot model to match the provided joint angles.
Args:
joint_positions (list): a list or flat numpy array of joint positions.
simulate (bool): If True, actually use physics simulation, else
write to physics state directly.
sync_last (bool): If False, don't sync the last joint angle. This
is useful for directly controlling the roll at the end effector.
"""
num_joints = len(joint_positions)
if not sync_last:
num_joints -= 1
for i in range(num_joints):
if simulate:
p.setJointMotorControl2(
self.ik_robot,
self.actual[i],
p.POSITION_CONTROL,
targetVelocity=0,
targetPosition=joint_positions[i],
force=500,
positionGain=0.5,
velocityGain=1.,
)
else:
# Note that we use self.actual[i], and not i
p.resetJointState(self.ik_robot, self.actual[i], joint_positions[i])
