def setJointPosition(self, robot, position, kp=1.0, kv=0.3):
import pybullet as p
num_joints = p.getNumJoints(robot)
zero_vec = [0.0] * num_joints
if len(position) == num_joints:
p.setJointMotorControlArray(robot, range(num_joints), p.POSITION_CONTROL,
targetPositions=position, targetVelocities=zero_vec,
positionGains=[kp] * num_joints, velocityGains=[kv] * num_joints)
def applyAction(self, actions):
forces = [0.] * len(self.motors)
for m in range(len(self.motors)):
forces[m] = self.motor_power[m]*actions[m]*0.082
p.setJointMotorControlArray(self.human, self.motors,controlMode=p.TORQUE_CONTROL, forces=forces)
p.stepSimulation()
time.sleep(0.01)
distance=5
yaw = 0
def setJointPosition(robot, position, kp=1.0, kv=0.3):
num_joints = p.getNumJoints(robot)
zero_vec = [0.0] * num_joints
if len(position) == num_joints:
p.setJointMotorControlArray(robot, range(num_joints), p.POSITION_CONTROL,
targetPositions=position, targetVelocities=zero_vec,
positionGains=[kp] * num_joints, velocityGains=[kv] * num_joints)
else:
print("Not setting torque. "
"Expected torque vector of "
"length {}, got {}".format(num_joints, len(torque)))
def take_step(self,
action,
robot_arm='left',
gains=0.05,
forces=1,
step_sim=True):
action = np.clip(action,
a_min=self.action_space.low,
a_max=self.action_space.high)
# print('cameraYaw=%.2f, cameraPitch=%.2f, distance=%.2f' % p.getDebugVisualizerCamera(physicsClientId=self.id)[-4:-1])
# print('Total time:', self.total_time)
# self.total_time += 0.1
self.iteration += 1
if self.last_sim_time is None:
self.last_sim_time = time.time()
action *= 0.05
action_robot = action
indices = self.robot_left_arm_joint_indices if robot_arm == 'left' else self.robot_right_arm_joint_indices if robot_arm == 'right' else self.robot_both_arm_joint_indices
robot_joint_states = p.getJointStates(self.robot,
jointIndices=indices,
physicsClientId=self.id)
robot_joint_positions = np.array([x[0] for x in robot_joint_states])
for _ in range(self.frame_skip):
action_robot[robot_joint_positions +
action_robot < self.robot_lower_limits] = 0
action_robot[robot_joint_positions +
action_robot > self.robot_upper_limits] = 0
robot_joint_positions += action_robot
p.setJointMotorControlArray(self.robot,
jointIndices=indices,
controlMode=p.POSITION_CONTROL,
targetPositions=robot_joint_positions,
positionGains=np.array(
[gains] * self.action_robot_len),
forces=[forces] * self.action_robot_len,
physicsClientId=self.id)
self.setup_record_video() # pierre
if step_sim:
# Update robot position
for _ in range(self.frame_skip):
p.stepSimulation(physicsClientId=self.id)
self.update_targets()
if self.gui:
# Slow down time so that the simulation matches real time
self.slow_time()
self.record_video_frame()
def _sendPositionCommand(self, commands):
''''''
num_motors = len(self.arm_joint_indices)
pb.setJointMotorControlArray(self.id,
self.arm_joint_indices,
pb.POSITION_CONTROL,
commands,
targetVelocities=[0.] * num_motors,
forces=[self.max_force] * num_motors,
positionGains=[self.position_gain] *
num_motors,
velocityGains=[1.0] * num_motors)
def set_pos(set_mode, position_list=[]):
maxForces = [10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10]
posGain = [0.01] * 12
#velGain = [0.1] * 12
p.setJointMotorControlArray(
quadruped,
jointIndices=motor_id_list,
controlMode=set_mode,
targetPositions=position_list,
#positionGains = posGain,
#velocityGains = velGain,
forces=maxForces)
def step(self):
# while True:
currj = [
p.getJointState(self.body, i)[0] for i in range(self.n_joints)
]
indj = [6, 3, 8, 5, 10]
targj = [currj[1], -currj[1], -currj[1], currj[1], currj[1]]
p.setJointMotorControlArray(self.body,
indj,
p.POSITION_CONTROL,
targj,
positionGains=np.ones(5))
def setJointPosition(self, robot, position, kp=1.0, kv=0.3):
import pybullet as p
num_joints = p.getNumJoints(robot)
zero_vec = [0.0] * num_joints
if len(position) == num_joints:
p.setJointMotorControlArray(robot,
range(num_joints),
p.POSITION_CONTROL,
targetPositions=position,
targetVelocities=zero_vec,
positionGains=[kp] * num_joints,
velocityGains=[kv] * num_joints)
def set_actuator_postions(self, joint_angles):
serial_joint_angles = self.parallel_to_serial_joint_angles(
joint_angles)
pybullet.setJointMotorControlArray(
bodyUniqueId=self.model[1],
jointIndices=self.joint_indices,
controlMode=pybullet.POSITION_CONTROL,
targetPositions=list(serial_joint_angles.T.reshape(12)),
positionGains=[self.kp] * 12,
velocityGains=[self.kv] * 12,
forces=[self.max_torque] * 12,
)
def pos(x1, y1, z1, roll0, yaw0, pitch0, robotID, eefID):
flag = True
# xin = p.addUserDebugParameter("x", -2, 2, x)
# yin = p.addUserDebugParameter("y", -2, 2, y)
# zin = p.addUserDebugParameter("z", -2, 2, z)
# roll1 = p.addUserDebugParameter("roll", -2*pi, 2*pi, roll)
# yaw1 = p.addUserDebugParameter("yaw", -2*pi, 2*pi, yaw)
# pitch1 = p.addUserDebugParameter("pitch", -2*pi, 2*pi, pitch)
# capture1 = p.addUserDebugParameter("capture",-3,3,capture)
while (flag):
# print('looping',flag)
# capture = p.readUserDebugParameter(capture1)
# ur5_camera(robotID)
# x = p.readUserDebugParameter(xin)
# y = p.readUserDebugParameter(yin)
# z = p.readUserDebugParameter(zin)
# roll = p.readUserDebugParameter(roll1)
# yaw = p.readUserDebugParameter(yaw1)
# pitch = p.readUserDebugParameter(pitch1)
x = x1
y = y1
z = z1
roll = roll0
yaw = yaw0
pitch = pitch0
robotEndOrn = p.getQuaternionFromEuler([roll, yaw, pitch])
jointPose = p.calculateInverseKinematics(robotID, eefID, [x, y, z],
robotEndOrn)
# print("---------------jointPose-------------------------")
# print(jointPose)
joint_index = [1, 2, 3, 4, 5, 6]
pose_group = [
jointPose[0], jointPose[1], jointPose[2], jointPose[3],
jointPose[4], jointPose[5]
]
p.setJointMotorControlArray(robotID,
joint_index,
p.POSITION_CONTROL,
targetPositions=pose_group)
rXYZ = p.getLinkState(robotID, eefID)[-2] # real XYZ
rWxWyWz = p.getLinkState(robotID, eefID)[-1]
# print(x,y,z,rXYZ,rWxWyWz)
Te = list(map(ABSE, [x, y, z], rXYZ)) #translation error
Qe = list(map(ABSE, robotEndOrn, rWxWyWz)) #rotation error
# print("x_err= {:.3f}, y_err= {:.3f}, z_err= {:.3f}".format(*Te))
# print("W1_err= {:.3f}, W2_err= {:.3f}, W3_err= {:.3f}, W4_err= {:.3f}".format(*Qe))
# for x in Te:
# print(x)
if (check(Te, Qe)):
# print('Te,Qe = ',Te,Qe)
flag = False
time.sleep(0.05)
p.stepSimulation()
def move_to_initial_position(self):
if self.save_sequence:
self.traj_indices.append(len(self.data))
pos_left = [2.5, 0.75, -0.4, 0, 0, 3.14]
pos_right = [-2.5, 0.75, -0.4, 0, 0, 3.14]
pb.setJointMotorControlArray(self.robot, self.joint_indices_left, pb.POSITION_CONTROL, targetPositions=pos_left, physicsClientId=self.pb_client_id)
pb.setJointMotorControlArray(self.robot, self.joint_indices_right, pb.POSITION_CONTROL, targetPositions=pos_right, physicsClientId=self.pb_client_id)
for s in range(100):
pb.stepSimulation(physicsClientId=self.pb_client_id)
if self.delay:
time.sleep(self.dt)
def set_joint_torques(side, torques):
if SIMULATION == True:
if side == Side.LEFT:
joints = [0, 1, 2, 3, 4]
if side == Side.RIGHT:
joints = [5, 6, 7, 8, 9]
for x in torques:
x = min(40, max(-40, x))
p.setJointMotorControlArray(bipedId,
joints,
p.TORQUE_CONTROL,
forces=torques)
def set_joint_torque(self, tau):
"""
sets joint torques to the values specified by the 1D numpy array tau
"""
assert (tau.shape[0] == self.num_joints)
zero_gains = tau.shape[0] * (0., )
p.setJointMotorControlArray(self.robot_id,
self.joint_ids,
p.TORQUE_CONTROL,
forces=tau,
positionGains=zero_gains,
velocityGains=zero_gains)
def __init__(self,
robot_id,
pinocchio_robot,
joint_names,
endeff_names,
useFixedBase=False):
self.nq = pinocchio_robot.nq
self.nv = pinocchio_robot.nv
self.nj = len(joint_names)
self.nf = len(endeff_names)
self.robot_id = robot_id
self.pinocchio_robot = pinocchio_robot
self.useFixedBase = useFixedBase
self.joint_names = joint_names
self.endeff_names = endeff_names
bullet_joint_map = {}
for ji in range(p.getNumJoints(robot_id)):
bullet_joint_map[p.getJointInfo(robot_id,
ji)[1].decode('UTF-8')] = ji
self.bullet_joint_ids = np.array(
[bullet_joint_map[name] for name in joint_names])
self.pinocchio_joint_ids = np.array(
[pinocchio_robot.model.getJointId(name) for name in joint_names])
self.pin2bullet_joint_only_array = []
if not self.useFixedBase:
for i in range(2, self.nj + 2):
self.pin2bullet_joint_only_array.append(
np.where(self.pinocchio_joint_ids == i)[0][0])
else:
for i in range(1, self.nj + 1):
self.pin2bullet_joint_only_array.append(
np.where(self.pinocchio_joint_ids == i)[0][0])
# Disable the velocity control on the joints as we use torque control.
p.setJointMotorControlArray(robot_id,
self.bullet_joint_ids,
p.VELOCITY_CONTROL,
forces=np.zeros(self.nj))
# In pybullet, the contact wrench is measured at a joint. In our case
# the joint is fixed joint. Pinocchio doesn't add fixed joints into the joint
# list. Therefore, the computation is done wrt to the frame of the fixed joint.
self.bullet_endeff_ids = [
bullet_joint_map[name] for name in endeff_names
]
self.pinocchio_endeff_ids = [
pinocchio_robot.model.getFrameId(name) for name in endeff_names
]
def send_joint_command(self, tau):
# TODO: Apply the torques on the base towards the simulator as well.
if not self.useFixedBase:
assert(tau.shape[0] == self.nv - 6)
else:
assert(tau.shape[0] == self.nv)
zeroGains = tau.shape[0] * (0.,)
p.setJointMotorControlArray(self.robot_id, self.bullet_joint_ids, p.TORQUE_CONTROL,
forces=tau[self.pin2bullet_joint_only_array],
positionGains=zeroGains, velocityGains=zeroGains)
def _assign_throttle(self, ee_target_pos, position_gain):
# Calculate joint positions using inverse kinematics
joint_pos = p.calculateInverseKinematics(self.botId, 6, ee_target_pos)
p.setJointMotorControlArray(bodyIndex=self.botId,
jointIndices=range(self.numJoints),
controlMode=p.POSITION_CONTROL,
targetPositions=joint_pos,
forces=[300] * self.numJoints,
positionGains=[position_gain] *
self.numJoints,
targetVelocities=[0.0] * self.numJoints,
velocityGains=[1] * self.numJoints)
def _send_move_command(self, jointPoses):
num_arm_indices = len(self.armInds)
p.setJointMotorControlArray(
self.armId,
self.armInds,
controlMode=p.POSITION_CONTROL,
targetPositions=jointPoses[:num_arm_indices],
targetVelocities=[0] * num_arm_indices,
# forces=[self.MAX_FORCE] * num_arm_indices,
forces=[100, 100, 60, 60, 50, 40, 40],
positionGains=[0.3] * num_arm_indices,
velocityGains=[1] * num_arm_indices)
def setController(self,
robotID,
controller_type,
joint_indices,
targetPositions=[None, None],
targetVelocities=None,
targetTorques=None):
if controller_type == 'velocity':
if targetPositions[0] != None:
p.setJointMotorControlArray(
robotID,
joint_indices,
p.VELOCITY_CONTROL,
targetPositions=targetPositions,
targetVelocities=targetVelocities,
velocityGains=np.array(
[1, 1, 1, 1, 1, 1,
1])) #, forces = [50]*len(targetVelocities))
else:
p.setJointMotorControlArray(
robotID,
joint_indices,
p.VELOCITY_CONTROL,
targetVelocities=targetVelocities
) #, velocityGains = np.array([1,1,1,1,1,1,1])*1)
elif controller_type == 'position':
p.setJointMotorControlArray(robotID,
joint_indices,
p.POSITION_CONTROL,
targetPositions=targetPositions,
targetVelocities=targetVelocities)
elif controller_type == 'torque':
if not self.torque_control_mode:
p.setJointMotorControlArray(robotID,
joint_indices,
p.VELOCITY_CONTROL,
forces=[0] * len(joint_indices))
#TODO: add a routine to read link indices to change this. BUG?
for link_idx in joint_indices:
p.changeDynamics(robotID,
link_idx,
linearDamping=0.0,
angularDamping=0.0,
jointDamping=0.0)
self.torque_control_mode = True
p.setJointMotorControlArray(robotID,
joint_indices,
p.TORQUE_CONTROL,
forces=targetTorques)
else:
print("unknown controller type")
def front_grasp(botId, rightCoords, leftCoords, data, steps=10, tableId1=None, tableId2=None, both=None, primitive_name=None, front_grasp=False):
revoluteJoints = getRevoluteJoints(botId)
x_prev_left = p.getLinkState(botId, 28)[0][0]
iter_left_x = (rightCoords[0]-x_prev_left)/steps
x_prev_right = p.getLinkState(botId, 51)[0][0]
iter_right_x = (rightCoords[0]-x_prev_right)/steps
y_prev_left = p.getLinkState(botId, 28)[0][1]
iter_left_y = (leftCoords[1]-y_prev_left)/steps
y_prev_right = p.getLinkState(botId, 51)[0][1]
iter_right_y = (rightCoords[1]-y_prev_right)/steps
i = 0
for i in range(steps):
x_prev_left += iter_left_x
x_prev_right += iter_right_x
y_prev_left += iter_left_y
y_prev_right += iter_right_y
gripperPosition1 = p.calculateInverseKinematics(botId, 28, [x_prev_left, y_prev_left, leftCoords[2]], maxNumIterations=1000)
gripperPosition2 = p.calculateInverseKinematics(botId, 51, [x_prev_right, y_prev_right, rightCoords[2]], maxNumIterations=1000)
jointStates = getJointStates(botId)
get_primitive_data(botId, data, primitive_name, sequence=1, tableId1=tableId1, tableId2=tableId2, front_grasp=front_grasp, both=both)
p.setJointMotorControlArray(botId,
jointIndices=revoluteJoints[0:10],
controlMode=p.POSITION_CONTROL,
targetPositions=gripperPosition1[0:10])
p.setJointMotorControlArray(botId,
jointIndices=revoluteJoints[10:],
controlMode=p.POSITION_CONTROL,
targetPositions=gripperPosition2[10:])
for i in range(10):
p.stepSimulation()
get_primitive_data(botId, data, primitive_name, sequence=2, tableId1=tableId1, tableId2=tableId2, front_grasp=front_grasp, both=both)
jointStates = getJointStates(botId)
return gripperPosition1, gripperPosition2
def configure_simulation(dt, enableGUI):
global jointTorques
# Load the robot for Pinocchio
solo = robots_loader.loadSolo(True)
solo.initViewer(loadModel=True)
# Start the client for PyBullet
if enableGUI:
physicsClient = p.connect(p.GUI)
else:
physicsClient = p.connect(p.DIRECT)
# p.GUI for graphical version
# p.DIRECT for non-graphical version
# Set gravity (disabled by default)
p.setGravity(0, 0, -9.81)
# Load horizontal plane for PyBullet
p.setAdditionalSearchPath(pybullet_data.getDataPath())
planeId = p.loadURDF("plane.urdf")
# Load the robot for PyBullet
robotStartPos = [0, 0, 0.35]
robotStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
p.setAdditionalSearchPath(
"/opt/openrobots/share/example-robot-data/robots/solo_description/robots"
)
robotId = p.loadURDF("solo.urdf", robotStartPos, robotStartOrientation)
# Set time step of the simulation
p.setTimeStep(dt)
# Disable default motor control for revolute joints
revoluteJointIndices = [0, 1, 3, 4, 6, 7, 9, 10]
p.setJointMotorControlArray(
robotId,
jointIndices=revoluteJointIndices,
controlMode=p.VELOCITY_CONTROL,
targetVelocities=[0.0 for m in revoluteJointIndices],
forces=[0.0 for m in revoluteJointIndices])
# Enable torque control for revolute joints
jointTorques = [0.0 for m in revoluteJointIndices]
p.setJointMotorControlArray(robotId,
revoluteJointIndices,
controlMode=p.TORQUE_CONTROL,
forces=jointTorques)
# Compute one step of simulation for initialization
p.stepSimulation()
return robotId, solo, revoluteJointIndices
def PDcontrol(q, qdot):
qError = desired_q - q
qdotError = desired_qdot - qdot
Kp = np.diagflat(kps)
Kd = np.diagflat(kds)
forces = Kp.dot(qError) + Kd.dot(qdotError)
force1 = np.clip(forces[0], -maxF[0], maxF[0])
force2 = np.clip(forces[1], -maxF[0], maxF[1])
forces = [force1, force2]
p.setJointMotorControlArray(pole, [0, 1],
controlMode=p.TORQUE_CONTROL,
forces=forces)
def __disable_pybullet_velocity_control(self):
"""
To disable the high friction velocity motors created by
default at all revolute and prismatic joints while loading them from
the urdf.
"""
pybullet.setJointMotorControlArray(
bodyUniqueId=self.finger_id,
jointIndices=self.pybullet_joint_indices,
controlMode=pybullet.VELOCITY_CONTROL,
targetVelocities=[0] * len(self.pybullet_joint_indices),
forces=[0] * len(self.pybullet_joint_indices),
)
def foot_position2motor_angle(self, foot_positions):
serial_joint_angles = kinematics.serial_quadruped_inverse_kinematics(
foot_positions, self.sim_hardware_config)
# print(serial_joint_angles)
pybullet.setJointMotorControlArray(
bodyUniqueId=self.body_id,
jointIndices=self.joint_indices,
controlMode=pybullet.POSITION_CONTROL,
targetPositions=list(serial_joint_angles.T.reshape(12)),
positionGains=[self.motor_kp] * 12,
velocityGains=[self.motor_kv] * 12,
forces=[self.motor_max_torque] * 12,
)
def set_q_pb(self, qd):
"""
Get the joint configuration of the robot arm.
Args:
qd (list): list of desired joint position
"""
pybullet.setJointMotorControlArray(
bodyUniqueId=self._arm_id,
jointIndices=range(0, self._num_joints),
controlMode=pybullet.POSITION_CONTROL,
targetPositions=qd,
physicsClientId=self.physics_id)
return qd
def set_motor_pos_vel(robot, joint_id, pos_cmd, vel_cmd):
pos_applied = OrderedDict()
vel_applied = OrderedDict()
for (joint_name, pos_des), (_, vel_des) in zip(pos_cmd.items(),
vel_cmd.items()):
pos_applied[joint_id[joint_name]] = pos_des
vel_applied[joint_id[joint_name]] = vel_des
p.setJointMotorControlArray(robot,
pos_applied.keys(),
controlMode=p.POSITION_CONTROL,
targetPositions=list(pos_applied.values()),
targetVelocities=list(vel_applied.values()))
def moveToPos(kukaID, finalPos, finalOrn, k=500):
num_joints = p.getNumJoints(kukaID)
finalAngles = p.calculateInverseKinematics(kukaID, 6, finalPos, finalOrn)
targetAngles = getAngleInterpolation(kukaID, finalAngles, k)
for i in range(k):
p.setJointMotorControlArray(bodyUniqueId=kukaID,
jointIndices=range(num_joints),
controlMode=p.POSITION_CONTROL,
targetPositions=targetAngles[i],
forces=num_joints * [100])
for j in range(ceil(500 / k)):
p.stepSimulation()
time.sleep(1. / 240.)
def simulateDyn(self, actions):
if self.SIM_READDY == 1:
p.setJointMotorControlArray(self.body_id,
self.ACTUATED_JOINTS_INDEX,
p.TORQUE_CONTROL,
forces=actions,
physicsClientId=self.client_id)
p.stepSimulation(physicsClientId=self.client_id)
if self.LOGDATA == 1:
self.actions_seq.append(actions)
self.observeState()
else:
raise Exception("Simulation not set up")
def applyAction(self, actions):
forces = [0.] * len(self.motors)
for m in range(len(self.motors)):
forces[m] = self.motor_power[m] * actions[m] * 0.082
p.setJointMotorControlArray(self.human,
self.motors,
controlMode=p.TORQUE_CONTROL,
forces=forces)
p.stepSimulation()
time.sleep(0.01)
distance = 5
yaw = 0
def setJointPosition(self, position, kp=1.0, kv=1.0):
print('Joint position controller')
zero_vec = [0.0] * len(self.controllable_joints)
p.setJointMotorControlArray(
self.robot_id,
self.controllable_joints,
p.POSITION_CONTROL,
targetPositions=position,
targetVelocities=zero_vec,
positionGains=[kp] * len(self.controllable_joints),
velocityGains=[kv] * len(self.controllable_joints))
for _ in range(100): # to settle the robot to its position
p.stepSimulation()
def send_joint_torque(self, tau):
"""
sends torque commands (needs an array of desired joint torques)
"""
assert (tau.shape[0] == self.nj)
zeroGains = tau.shape[0] * (0., )
p.setJointMotorControlArray(self.robotId,
self.bullet_joint_ids,
p.TORQUE_CONTROL,
forces=tau,
positionGains=zeroGains,
velocityGains=zeroGains)
def hold(flag):
if flag:
curr_joint_value[7] = 1
curr_joint_value[8] = 1
else:
curr_joint_value[7] = 0
curr_joint_value[8] = 0
p.setJointMotorControlArray(kukaId,
range(11),
p.POSITION_CONTROL,
targetPositions=curr_joint_value)
time.sleep(1)
def full_code():
p.connect(p.GUI)
p.setGravity(0, 0, -9.8)
_ = p.loadURDF(os.path.join(MODELS_DIR, PLANE_URDF))
r2d2 = p.loadURDF(os.path.join(MODELS_DIR, R2D2_URDF), 0, 0, 0.5)
p.setJointMotorControlArray(r2d2, R2D2_WHEEL_JOINTS, p.VELOCITY_CONTROL,
[1, 1, 1, 1], [1, 1, 1, 1])
for ts in xrange(10000):
print(ts)
for i in xrange(p.getNumJoints(r2d2)):
print(i, p.getJointState(r2d2, i))
p.stepSimulation()
time.sleep(.0001)
def apply_action_ik(target_ee_pos,
target_ee_quat,
target_gripper_state,
robot_id,
end_effector_index,
movable_joints,
lower_limit,
upper_limit,
rest_pose,
joint_range,
num_sim_steps=5):
joint_poses = p.calculateInverseKinematics(robot_id,
end_effector_index,
target_ee_pos,
target_ee_quat,
lowerLimits=lower_limit,
upperLimits=upper_limit,
jointRanges=joint_range,
restPoses=rest_pose,
jointDamping=[0.001] *
len(movable_joints),
solver=0,
maxNumIterations=100,
residualThreshold=.01)
p.setJointMotorControlArray(
robot_id,
movable_joints,
controlMode=p.POSITION_CONTROL,
targetPositions=joint_poses,
# targetVelocity=0,
forces=[5] * len(movable_joints),
positionGains=[0.03] * len(movable_joints),
# velocityGain=1
)
# set gripper action
p.setJointMotorControl2(robot_id,
movable_joints[-2],
controlMode=p.POSITION_CONTROL,
targetPosition=target_gripper_state[0],
force=500,
positionGain=0.03)
p.setJointMotorControl2(robot_id,
movable_joints[-1],
controlMode=p.POSITION_CONTROL,
targetPosition=target_gripper_state[1],
force=500,
positionGain=0.03)
for _ in range(num_sim_steps):
p.stepSimulation()
id_robot = bullet.loadURDF("TwoJointRobot_w_fixedJoints.urdf",
robot_base, robot_orientation, useFixedBase=True)
else:
id_revolute_joints = [0, 1]
id_robot = bullet.loadURDF("TwoJointRobot_wo_fixedJoints.urdf",
robot_base, robot_orientation, useFixedBase=True)
bullet.changeDynamics(id_robot,-1,linearDamping=0, angularDamping=0)
bullet.changeDynamics(id_robot,0,linearDamping=0, angularDamping=0)
bullet.changeDynamics(id_robot,1,linearDamping=0, angularDamping=0)
jointTypeNames = ["JOINT_REVOLUTE", "JOINT_PRISMATIC","JOINT_SPHERICAL","JOINT_PLANAR","JOINT_FIXED","JOINT_POINT2POINT","JOINT_GEAR"]
# Disable the motors for torque control:
bullet.setJointMotorControlArray(id_robot, id_revolute_joints, bullet.VELOCITY_CONTROL, forces=[0.0, 0.0])
# Target Positions:
start = 0.0
end = 1.0
steps = int((end-start)/delta_t)
t = [0]*steps
q_pos_desired = [[0.]* steps,[0.]* steps]
q_vel_desired = [[0.]* steps,[0.]* steps]
q_acc_desired = [[0.]* steps,[0.]* steps]
for s in range(steps):
t[s] = start+s*delta_t
q_pos_desired[0][s] = 1./(2.*math.pi) * math.sin(2. * math.pi * t[s]) - t[s]
q_pos_desired[1][s] = -1./(2.*math.pi) * (math.cos(2. * math.pi * t[s]) - 1.0)
