def __call__(self, obs, *args, **kwargs):
torque = p.calculateInverseDynamics(self.id,
list(obs['robot_position']),
list(obs['robot_velocity']),
[0. for _ in range(9)])
tas = TriFingerPlatform.spaces.robot_torque.gym
return np.clip(np.array(torque), tas.low, tas.high)
def update(self, action):
joint_positions = [
state[0] for state in p.getJointStates(self.uid, self.joint_ids)
]
joint_velocities = [
state[1] for state in p.getJointStates(self.uid, self.joint_ids)
]
n_joints = len(joint_positions)
J = p.calculateJacobian(self.uid, self.end_frame,
self.offset_admittance_point, joint_positions,
[0.] * n_joints, [0.] * n_joints)
T_g = p.calculateInverseDynamics(self.uid, joint_positions,
[0.] * n_joints, [0.] * n_joints)
T_cmd = action['force'].dot(np.array(J[0])) + action['torque'].dot(
np.array(J[1]))
T_pos = (self.target_pose - np.array(joint_positions)) * self.kp
T_vel = (np.array(joint_velocities)) * -self.kd
p.setJointMotorControlArray(self.uid,
self.joint_ids,
p.TORQUE_CONTROL,
forces=T_cmd + T_g + T_pos + T_vel)
def _get_gravcomp(self, observation):
# Returns: 9 torques required for grav comp
ret = pybullet.calculateInverseDynamics(self.finger.finger_id,
observation["observation"]["position"].tolist(),
np.zeros(len(observation["observation"]["position"])).tolist(),
np.zeros(len(observation["observation"]["position"])).tolist())
ret = np.array(ret)
return ret
def get_bullet_fd(joint_pos, joint_vel, joint_acc):
# Update joint damping
for link_idx in range(sim.num_joints):
if use_damping:
joint_damping = robot_model._bodies[
link_idx + 1
].get_joint_damping_const()
else:
joint_damping = 0.0
p.changeDynamics(
sim.robot_id,
link_idx,
linearDamping=0.0,
angularDamping=0.0,
jointDamping=joint_damping,
physicsClientId=sim.pc_id,
)
p.setJointMotorControlArray( # activating torque control
bodyIndex=sim.robot_id,
jointIndices=controlled_joints,
controlMode=p.VELOCITY_CONTROL,
forces=np.zeros(num_dofs),
physicsClientId=sim.pc_id,
)
set_pybullet_state(sim, robot_model, num_dofs, joint_pos, joint_vel)
bullet_tau = np.array( # torque that achieves joint_acc from current state
p.calculateInverseDynamics(
sim.robot_id,
joint_pos,
joint_vel,
joint_acc,
physicsClientId=sim.pc_id,
)
)
p.setJointMotorControlArray(
bodyIndex=sim.robot_id,
jointIndices=controlled_joints,
controlMode=p.TORQUE_CONTROL,
forces=bullet_tau,
physicsClientId=sim.pc_id,
)
p.stepSimulation(physicsClientId=sim.pc_id)
cur_joint_states = p.getJointStates(
sim.robot_id, controlled_joints, physicsClientId=sim.pc_id
)
q = [cur_joint_states[i][0] for i in range(num_dofs)]
qd = [cur_joint_states[i][1] for i in range(num_dofs)]
qdd = (np.array(qd) - np.array(joint_vel)) / dt
return bullet_tau, qdd
def get_bullet_invdyn(joint_pos, joint_vel, joint_acc):
set_pybullet_state(sim, robot_model, num_dofs, joint_pos, joint_vel)
return p.calculateInverseDynamics(
sim.robot_id,
joint_pos,
joint_vel,
joint_acc,
physicsClientId=sim.pc_id,
)
def main():
# reset env
physicsClient = p.connect(p.GUI) # or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) # optionally
p.setGravity(0, 0, -9.81)
planeId = p.loadURDF("plane.urdf")
# reset robot
robotStartPos = [0, 0, 0]
robotStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
robotId = p.loadURDF(
"/home/xin/codes/bullet3/pybullet/new_examples/model/rrbot.urdf",
robotStartPos, robotStartOrientation)
print("-" * 50)
print("robotId: ", robotId)
N = p.getNumJoints(robotId)
n = N - 1
print("NumJoints: ", n)
for i in range(p.getNumJoints(robotId)):
print(p.getJointInfo(robotId, i))
jointIds = [1, 2]
p.setJointMotorControlArray(robotId,
jointIds,
p.VELOCITY_CONTROL,
forces=[0.0, 0.0])
p.setJointMotorControlArray(robotId,
jointIds,
p.TORQUE_CONTROL,
forces=[0.0, 0.0])
# start simulation
while True:
# sense robot state
robotPos, robotOrn = p.getBasePositionAndOrientation(robotId)
print("robot state: ", robotPos, robotOrn)
joint_states = p.getJointStates(robotId, jointIds)
print("joint state: ", joint_states)
q = [joint_state[0] for joint_state in joint_states]
qd = [joint_state[1] for joint_state in joint_states]
print("q: ", q)
print("qd: ", qd)
# controller
qd_des = [0] * n # gravity compensation
torque_cmd = p.calculateInverseDynamics(robotId, q, qd, qd_des)
p.setJointMotorControlArray(robotId,
jointIds,
p.TORQUE_CONTROL,
forces=torque_cmd)
# sim one step
p.stepSimulation()
time.sleep(1. / 240.)
def computeInverseDynamics(self, robotID, positions, velocities,
accelerations):
""" Use pybullet function to compute the inverse dynamics of the given robot
"""
# print("computing inverse dynamics")
jointTorques = p.calculateInverseDynamics(robotID, positions,
velocities, accelerations)
# print("Finished computing inverse dynamics")
return jointTorques
def _get_gravcomp(self, observation):
# Returns: 9 torques required for grav comp
ret = p.calculateInverseDynamics(
self.env.platform.simfinger.finger_id,
observation["robot_position"].tolist(),
observation["robot_velocity"].tolist(),
np.zeros(len(observation["robot_position"])).tolist(),
self.env.platform.simfinger._pybullet_client_id)
ret = np.array(ret)
return ret
def N_q(self):
"""Joint space gravity vector.
"""
Nq = pybullet.calculateInverseDynamics(
bodyUniqueId=self._arm_id,
objPositions=self.motor_joint_positions,
objVelocities=self.motor_joint_velocities,
objAccelerations=[0] * len(self.motor_joint_positions),
physicsClientId=self._physics_id)
return np.asarray(Nq)[:7]
def GetCoriolosMatrix(self, q, dq):
'''Compute C(q, q dot) by calling the inverse dynamics function with
zero acceleration. The full set of joints (and hence inverse dynamics
equations) include the finger joints, so we include them and then
remove them after. Also, the inverse dynamics function cannot take
numpy array and must be fed lists.
@param q Configuration
@param dq joint velocities
@return Coriolis vector, Nx1'''
q_plus = numpy.append(q, [0, 0]).tolist()
dq_plus = numpy.append(dq, [0, 0]).tolist()
ddq = [0.0]*9
coriolis = p.calculateInverseDynamics(self.__robot.id, q_plus, dq_plus, ddq)[0:7]
return coriolis
def doInverseDynamics(self, th_initial, th_final, final_time=2):
p.setRealTimeSimulation(False)
# get the desired trajectory
q_d, dq_d, ddq_d = self.getTrajectory(th_initial,
th_final,
tf=final_time,
dt=self.time_step)
traj_points = q_d.shape[0]
print('#Trajectory points:', traj_points)
# forward dynamics simulation loop
# for turning off link and joint damping
for link_idx in range(self.num_joints + 1):
p.changeDynamics(self.robot_id,
link_idx,
linearDamping=0.0,
angularDamping=0.0,
jointDamping=0.0)
p.changeDynamics(self.robot_id, link_idx, maxJointVelocity=200)
# Enable torque control
p.setJointMotorControlArray(self.robot_id,
self.controllable_joints,
p.VELOCITY_CONTROL,
forces=np.zeros(
len(self.controllable_joints)))
kd = 0.7 # from URDF file
n = 0
while n < traj_points:
tau = p.calculateInverseDynamics(self.robot_id, list(q_d[n]),
list(dq_d[n]), list(ddq_d[n]))
# tau += kd * dq_d[n] #if joint damping is turned off, this torque will not be required
# print(tau)
# torque control
p.setJointMotorControlArray(self.robot_id,
self.controllable_joints,
controlMode=p.TORQUE_CONTROL,
forces=tau)
theta, _, _ = self.getJointStates()
print('n:{}::th:{}'.format(n, theta))
p.stepSimulation()
time.sleep(self.time_step)
n += 1
print('Desired joint angles:', th_final)
p.disconnect()
def test_inverse_dynamics(self, request, setup_dict):
robot_model = setup_dict["robot_model"]
test_case = setup_dict["test_case"]
test_angles, test_velocities = (
test_case["joint_angles"],
test_case["joint_velocities"],
)
test_accelerations = test_case["joint_accelerations"]
for i in range(dof):
p.resetJointState(bodyUniqueId=robot_id,
jointIndex=i,
targetValue=test_angles[i],
targetVelocity=test_velocities[i],
physicsClientId=pc_id)
bullet_torques = p.calculateInverseDynamics(robot_id,
test_angles,
test_velocities,
test_accelerations,
physicsClientId=pc_id)
model_torques = robot_model.compute_inverse_dynamics(
torch.Tensor(test_angles).reshape(1, dof),
torch.Tensor(test_velocities).reshape(1, dof),
torch.Tensor(test_accelerations).reshape(1, dof),
include_gravity=True,
)
if JOINT_DAMPING != 0.0:
# if we have non-zero joint damping, we'll have to subtract the damping term from our predicted torques,
# because pybullet does not include damping/viscous friction in their inverse dynamics call
damping_const = torch.zeros(1, robot_model._n_dofs)
qd = torch.Tensor(test_velocities).reshape(1, dof)
for i in range(robot_model._n_dofs):
idx = robot_model._controlled_joints[i]
damping_const[:, i] = robot_model._bodies[
idx].get_joint_damping_const()
damping_term = damping_const.repeat(1, 1) * qd
model_torques -= damping_term
assert np.allclose(
model_torques.detach().squeeze().numpy(),
np.asarray(bullet_torques),
atol=1e-7,
)
def test_inverse_dynamics(self, request, setup_dict, use_damping):
robot_model, sim, num_dofs, test_case = extract_setup_dict(setup_dict)
# Bullet sim
set_pybullet_state(sim, robot_model, num_dofs, test_case.joint_pos,
test_case.joint_vel)
bullet_torques = p.calculateInverseDynamics(
sim.robot_id,
test_case.joint_pos,
test_case.joint_vel,
test_case.joint_acc,
physicsClientId=sim.pc_id,
)
# Differentiable model
model_torques = robot_model.compute_inverse_dynamics(
torch.Tensor(test_case.joint_pos).reshape(1, num_dofs),
torch.Tensor(test_case.joint_vel).reshape(1, num_dofs),
torch.Tensor(test_case.joint_acc).reshape(1, num_dofs),
include_gravity=True,
use_damping=use_damping,
)
if use_damping:
# if we have non-zero joint damping, we'll have to subtract the damping term from our predicted torques,
# because pybullet does not include damping/viscous friction in their inverse dynamics call
damping_const = torch.zeros(1, num_dofs)
qd = torch.Tensor(test_case.joint_vel).reshape(1, num_dofs)
for i in range(robot_model._n_dofs):
idx = robot_model._controlled_joints[i]
damping_const[:, i] = robot_model._bodies[
idx].get_joint_damping_const()
damping_term = damping_const.repeat(1, 1) * qd
model_torques -= damping_term
# Compare
assert np.allclose(
model_torques.detach().squeeze().numpy(),
np.asarray(bullet_torques),
atol=1e-7,
)
def gravity_vector(self):
"""Compute the gravity vector at the current joint state.
Pybullet does not currently expose the gravity vector so we compute it
using inverse dynamics.
Args : None
Returns:
gravity_torques (list): num_joints x 1 list of gravity torques on each joint.
Notes: to ignore coriolis forces we set the object velocities to zero.
"""
gravity_torques = pybullet.calculateInverseDynamics(
bodyUniqueId=self._arm_id,
objPositions=self.motor_joint_positions,
objVelocities=[0] * len(self.motor_joint_positions),
objAccelerations=[0] * len(self.motor_joint_positions),
physicsClientId=self._physics_id)
return gravity_torques[:7]
def RBD_B(self):
"""
The is the non_linear_effects of the rigid body dynamics
RBD_A ddq + RBD_B = torque + J.T F
"""
return np.array(p.calculateInverseDynamics(robot, list(self.state[:GP.QDIMS]), list(self.state[GP.QDIMS:]), [0.0] * GP.QDIMS))
obj_pos = [q_pos[0][i], q_pos[1][i]]
obj_vel = [q_vel[0][i], q_vel[1][i]]
obj_acc = [q_acc_desired[0][i], q_acc_desired[1][i]]
if (verbose):
print("calculateInverseDynamics")
print("id_robot")
print(id_robot)
print("obj_pos")
print(obj_pos)
print("obj_vel")
print(obj_vel)
print("obj_acc")
print(obj_acc)
torque = bullet.calculateInverseDynamics(id_robot, obj_pos, obj_vel, obj_acc)
q_tor[0][i] = torque[0]
q_tor[1][i] = torque[1]
if (verbose):
print("torque=")
print(torque)
# Set the Joint Torques:
bullet.setJointMotorControlArray(id_robot, id_revolute_joints, bullet.TORQUE_CONTROL, forces=[torque[0], torque[1]])
# Step Simulation
bullet.stepSimulation()
# Plot the Position, Velocity and Acceleration:
if plot:
figure = plt.figure(figsize=[15, 4.5])
obj_pos = [q_pos[0][i], q_pos[1][i]]
obj_vel = [q_vel[0][i], q_vel[1][i]]
obj_acc = [q_acc_desired[0][i], q_acc_desired[1][i]]
if (verbose):
print("calculateInverseDynamics")
print("id_robot")
print(id_robot)
print("obj_pos")
print(obj_pos)
print("obj_vel")
print(obj_vel)
print("obj_acc")
print(obj_acc)
torque = bullet.calculateInverseDynamics(id_robot, obj_pos, obj_vel,
obj_acc)
q_tor[0][i] = torque[0]
q_tor[1][i] = torque[1]
if (verbose):
print("torque=")
print(torque)
# Set the Joint Torques:
bullet.setJointMotorControlArray(id_robot,
id_revolute_joints,
bullet.TORQUE_CONTROL,
forces=[torque[0], torque[1]])
# Step Simulation
bullet.stepSimulation()
jointIndex=wheel_index,
force=calculate_approx_motor_torque(),
controlMode=p.VELOCITY_CONTROL,
targetVelocity=front_speed
)
while True:
# p.stepSimulation()
car_pos, car_orientation = p.getBasePositionAndOrientation(car_id)
torques = stabilization_control()
test = [0, 0, 0, 0]
print('test')
print(p.calculateInverseDynamics(car_id, test, test, test))
attitude_error()
# p.setJointMotorControl2(
# bodyUniqueId=car_id,
# jointIndex=0, # front left
# controlMode=p.VELOCITY_CONTROL,
# targetVelocity=100
# )
# front_speed, back_speed = gyro_control_update(math.degrees(p.getEulerFromQuaternion(car_orientation)[1]))
# print(p.getEulerFromQuaternion(car_orientation))
# front_speed = 0#100
# back_speed = 100
# set_motors(front_speed, back_speed)
return cs.Function("temp",[],[asd])()["o0"].toarray()
def list2np(asd):
return np.asarray(asd)
n_itr = 1000
for i in range(n_itr):
for j in range(n_joints):
q[j] = (q_max[j] - q_min[j])*np.random.rand()-(q_max[j] - q_min[j])/2
q_kdl[j] = q[j]
q_np[j] = q[j]
zeros_pb[j] = 0.
rbdl.InverseDynamics(urmodel, q_np, zeros_rbdl, zeros_rbdl, g_rbdl)
kdl.ChainDynParam(ur_chain, gravity_kdl).JntToGravity(q_kdl, g_kdl)
g_pb = pb.calculateInverseDynamics(pbmodel, q, zeros_pb, zeros_pb)
g_u2c = g_sym(q)
#print g_u2c
for tau_idx in range(n_joints):
error_kdl_rbdl[tau_idx] += np.absolute((list2np(g_kdl[tau_idx]) - g_rbdl[tau_idx]))
error_kdl_u2c[tau_idx] += np.absolute((list2np(g_kdl[tau_idx]) - u2c2np(g_u2c[tau_idx])))
error_rbdl_u2c[tau_idx] += np.absolute((u2c2np(g_u2c[tau_idx]) - g_rbdl[tau_idx]))
error_pb_u2c[tau_idx] += np.absolute((u2c2np(g_u2c[tau_idx]) - list2np(g_pb[tau_idx])))
error_pb_kdl[tau_idx] += np.absolute((list2np(g_kdl[tau_idx]) - list2np(g_pb[tau_idx])))
error_pb_rbdl[tau_idx] += np.absolute(g_rbdl[tau_idx] - list2np(g_pb[tau_idx]))
sum_error_kdl_rbdl = 0
sum_error_kdl_u2c = 0
sum_error_rbdl_u2c = 0
def inverse_dynamics(self, obs, id):
torque = p.calculateInverseDynamics(id, list(obs['robot_position']),
list(obs['robot_velocity']),
[0. for _ in range(9)])
return np.array(torque)
def calculate_inverse_dynamics(self, *args, **kwargs):
return pb.calculateInverseDynamics(*args,
**kwargs,
physicsClientId=self.bullet_cid)
n_itr = 1000
for i in range(n_itr):
for j in range(n_joints):
q[j] = (q_max[j] - q_min[j]) * np.random.rand() - (q_max[j] -
q_min[j]) / 2
qdot[j] = (q_max[j] - q_min[j]) * np.random.rand() - (q_max[j] -
q_min[j]) / 2
qddot[j] = (q_max[j] - q_min[j]) * np.random.rand() - (q_max[j] -
q_min[j]) / 2
q_np[j] = q[j]
qdot_np[j] = qdot[j]
qddot_np[j] = qddot[j]
rbdl.InverseDynamics(ur5_rbdl, q_np, qdot_np, qddot_np, id_rbdl)
id_pb = pb.calculateInverseDynamics(ur5_pb, q, qdot, qddot)
id_u2c = id_sym(q, qdot, qddot)
for tau_idx in range(n_joints):
error_pb_u2c[tau_idx] += np.absolute(
list2np(id_pb[tau_idx]) - u2c2np(id_u2c[tau_idx]))
error_rbdl_pb[tau_idx] += np.absolute(id_rbdl[tau_idx] -
list2np(id_pb[tau_idx]))
error_rbdl_u2c[tau_idx] += np.absolute(id_rbdl[tau_idx] -
u2c2np(id_u2c)[tau_idx])
sum_error_rbdl_pb = 0
sum_error_rbdl_u2c = 0
sum_error_pb_u2c = 0
for err in range(n_joints):
q_kdl[j] = q[j]
q_np[j] = q[j]
qdot[j] = (q_max[j] - q_min[j]) * np.random.rand() - (q_max[j] -
q_min[j]) / 2
qdot_kdl[j] = qdot[j]
qdot_np[j] = qdot[j]
zeros_pb[j] = 0.
rbdl.NonlinearEffects(snake_rbdl, q_np, qdot_np, C_rbdl)
kdl.ChainDynParam(snake_chain, gravity_kdl).JntToGravity(q_kdl, g_kdl)
kdl.ChainDynParam(snake_chain,
gravity_kdl).JntToCoriolis(q_kdl, qdot_kdl, C_kdl)
pb.setGravity(0, 0, 0)
C_pb = pb.calculateInverseDynamics(snake_pb, q, qdot, zeros_pb)
pb.setGravity(0, 0, -9.81)
g_pb = pb.calculateInverseDynamics(snake_pb, q, zeros_pb, zeros_pb)
g_u2c = g_sym(q)
C_u2c = C_sym(q, qdot)
for tau_idx in range(n_joints):
error_kdl_rbdl[tau_idx] += np.absolute(
((list2np(g_kdl[tau_idx]) + list2np(C_kdl[tau_idx])) -
C_rbdl[tau_idx]))
error_kdl_u2c[tau_idx] += np.absolute(
(list2np(C_kdl[tau_idx]) - u2c2np(C_u2c[tau_idx])))
error_rbdl_u2c[tau_idx] += np.absolute(
((u2c2np(g_u2c[tau_idx]) + u2c2np(C_u2c)[tau_idx]) -
C_rbdl[tau_idx]))
return np.asarray(asd)
n_itr = 1000
for i in range(n_itr):
for j in range(n_joints):
q[j] = (q_max[j] - q_min[j])*np.random.rand()-(q_max[j] - q_min[j])/2
qdot[j] = (q_max[j] - q_min[j])*np.random.rand()-(q_max[j] - q_min[j])/2
qddot[j] = (q_max[j] - q_min[j])*np.random.rand()-(q_max[j] - q_min[j])/2
q_np[j] = q[j]
qdot_np[j] = qdot[j]
qddot_np[j] = qddot[j]
rbdl.InverseDynamics(gantry_rbdl, q_np, qdot_np, qddot_np, id_rbdl)
id_pb = pb.calculateInverseDynamics(gantry_pb, q, qdot, qddot)
id_u2c = id_sym(q, qdot, qddot)
for tau_idx in range(n_joints):
error_pb_u2c[tau_idx] += np.absolute(list2np(id_pb[tau_idx]) - u2c2np(id_u2c[tau_idx]))
error_rbdl_pb[tau_idx] += np.absolute(id_rbdl[tau_idx] - list2np(id_pb[tau_idx]))
error_rbdl_u2c[tau_idx] += np.absolute(id_rbdl[tau_idx] - u2c2np(id_u2c)[tau_idx])
sum_error_rbdl_pb = 0
sum_error_rbdl_u2c = 0
sum_error_pb_u2c = 0
for err in range(n_joints):
sum_error_rbdl_u2c += error_rbdl_u2c[err]
sum_error_rbdl_pb += error_rbdl_pb[err]
def test_forward_dynamics(self, request, setup_dict):
robot_model = setup_dict["robot_model"]
test_case = setup_dict["test_case"]
test_angles, test_velocities = (
test_case["joint_angles"],
test_case["joint_velocities"],
)
test_accelerations = test_case["joint_accelerations"]
dt = 1.0 / 240.0
n_dofs = dof
controlled_joints = range(n_dofs)
# activating torque control
p.setJointMotorControlArray(bodyIndex=robot_id,
jointIndices=controlled_joints,
controlMode=p.VELOCITY_CONTROL,
forces=np.zeros(n_dofs),
physicsClientId=pc_id)
# set simulation to be in state test_angles/test_velocities
for i in range(dof):
p.resetJointState(bodyUniqueId=robot_id,
jointIndex=i,
targetValue=test_angles[i],
targetVelocity=test_velocities[i],
physicsClientId=pc_id)
# let's get the torque that achieves the test_accelerations from the current state
bullet_tau = np.array(
p.calculateInverseDynamics(robot_id,
test_angles,
test_velocities,
test_accelerations,
physicsClientId=pc_id))
p.setJointMotorControlArray(bodyIndex=robot_id,
jointIndices=controlled_joints,
controlMode=p.TORQUE_CONTROL,
forces=bullet_tau,
physicsClientId=pc_id)
p.stepSimulation(physicsClientId=pc_id)
cur_joint_states = p.getJointStates(robot_id,
controlled_joints,
physicsClientId=pc_id)
q = [cur_joint_states[i][0] for i in range(n_dofs)]
qd = [cur_joint_states[i][1] for i in range(n_dofs)]
qdd = (np.array(qd) - np.array(test_velocities)) / dt
model_qdd = robot_model.compute_forward_dynamics(
torch.Tensor(test_angles).reshape(1, dof),
torch.Tensor(test_velocities).reshape(1, dof),
torch.Tensor(bullet_tau).reshape(1, dof),
include_gravity=True,
use_damping=use_damping)
model_qdd = np.asarray(model_qdd.detach().squeeze())
if JOINT_DAMPING == 0.0:
# we can only test this if joint damping is zero,
# if it is non-zero the pybullet forward dynamics and inverse dynamics call will not be exactly the
# "inverse" of each other
assert np.allclose(model_qdd,
np.asarray(test_accelerations),
atol=1e-7) # if atol = 1e-3 it doesnt pass
assert np.allclose(model_qdd, qdd,
atol=1e-7) # if atol = 1e-3 it doesnt pass
M = p.calculateMassMatrix(model.Id, model.get_joints_pos_tuple())
M = np.array(M)
Ma = M[:, model.mask_act][model.mask_act, :]
print(np.round(M, 3))
print("DIM M = ", M.shape)
print(np.round(Ma, 3))
print("DIM Ma = ", Ma.shape)
###
q_list = list(model.get_joints_pos_tuple()) + ([0] * 6)
qd_list = list(model.get_joints_speeds_tuple())
qd_list = [1] * (model.num_joints + 6)
qdd_des_list = [1] * (model.num_joints + 6)
tau_exact = np.array(
p.calculateInverseDynamics(model.Id,
q_list,
qd_list,
qdd_des_list,
flags=1))
print("TAU EXACT\n", tau_exact)
print("Q = ", model.get_q())
model.set_velocities()
p.stepSimulation()
time.sleep(dt)
model.set_velocities()
print("qd_prev", model.qd_prev)
print("qd_curr", model.qd_curr)
qdd_history = np.array(([0] * (model.num_joints + 6)))
qdd_f_history = np.array(([0] * (model.num_joints + 6)))
model.turn_off_crawler()
for i in range(20):
p.stepSimulation()
# Run Sim
t = 0
count = 0
while (1):
# Get SensorData
sensor_data = pybullet_util.get_sensor_data(robot, joint_id, link_id,
pos_basejoint_to_basecom,
rot_basejoint_to_basecom)
# pb dynamics quantity
mass_pb = np.array(
p.calculateMassMatrix(robot,
list(sensor_data['joint_pos'].values())))
bg_pb = p.calculateInverseDynamics(
robot, list(sensor_data['joint_pos'].values()),
list(sensor_data['joint_vel'].values()), [0., 0.])
# print("mass pb")
# print(mass_pb)
# print("cori + grav pb")
# print(bg_pb)
# given sensor data compute inv dyn
# pybullet_id_result_list.append(
# p.calculateInverseDynamics(robot,
# list(sensor_data['joint_pos'].values()),
# list(sensor_data['joint_vel'].values()),
# list(CONSTANT_DES_ACC)))
time_list.append(t)
q[j] = (q_max[j] - q_min[j])*np.random.rand()-(q_max[j] - q_min[j])/2
q_kdl[j] = q[j]
q_np[j] = q[j]
qdot[j] = (q_max[j] - q_min[j])*np.random.rand()-(q_max[j] - q_min[j])/2
qdot_kdl[j] = qdot[j]
qdot_np[j] = qdot[j]
zeros_pb[j] = 0.
rbdl.NonlinearEffects(gantry_rbdl, q_np, qdot_np, C_rbdl)
kdl.ChainDynParam(gantry_kdl, gravity_kdl).JntToGravity(q_kdl, g_kdl)
kdl.ChainDynParam(gantry_kdl, gravity_kdl).JntToCoriolis(q_kdl, qdot_kdl, C_kdl)
pb.setGravity(0, 0, 0)
C_pb = pb.calculateInverseDynamics(gantry_pb, q, qdot, zeros_pb)
pb.setGravity(0, 0, -9.81)
g_pb = pb.calculateInverseDynamics(gantry_pb, q, zeros_pb, zeros_pb)
g_u2c = g_sym(q)
C_u2c = C_sym(q, qdot)
for tau_idx in range(n_joints):
error_kdl_rbdl[tau_idx] += np.absolute(((list2np(g_kdl[tau_idx]) + list2np(C_kdl[tau_idx])) - C_rbdl[tau_idx]))
error_kdl_u2c[tau_idx] += np.absolute((list2np(C_kdl[tau_idx]) - u2c2np(C_u2c[tau_idx])))
error_rbdl_u2c[tau_idx] += np.absolute(((u2c2np(g_u2c[tau_idx]) + u2c2np(C_u2c)[tau_idx]) - C_rbdl[tau_idx]))
error_pb_u2c[tau_idx] += np.absolute((u2c2np(C_u2c[tau_idx]) - list2np(C_pb[tau_idx])))
error_pb_kdl[tau_idx] += np.absolute((list2np(C_kdl[tau_idx]) - list2np(C_pb[tau_idx])))
error_pb_rbdl[tau_idx] += np.absolute(C_rbdl[tau_idx] - (list2np(C_pb[tau_idx]) + list2np(g_pb[tau_idx])))
def calcID(self, pos, vel, acc):
return p.calculateInverseDynamics(self.robotId, pos, vel, acc)
n_itr = 1000
for i in range(n_itr):
for j in range(n_joints):
q[j] = (q_max[j] - q_min[j]) * np.random.rand() - (q_max[j] -
q_min[j]) / 2
qdot[j] = (q_max[j] - q_min[j]) * np.random.rand() - (q_max[j] -
q_min[j]) / 2
qddot[j] = (q_max[j] - q_min[j]) * np.random.rand() - (q_max[j] -
q_min[j]) / 2
q_np[j] = q[j]
qdot_np[j] = qdot[j]
qddot_np[j] = qddot[j]
rbdl.InverseDynamics(snake_rbdl, q_np, qdot_np, qddot_np, id_rbdl)
id_pb = pb.calculateInverseDynamics(snake_pb, q, qdot, qddot)
id_u2c = id_sym(q, qdot, qddot)
for tau_idx in range(n_joints):
error_pb_u2c[tau_idx] += np.absolute(
list2np(id_pb[tau_idx]) - u2c2np(id_u2c[tau_idx]))
error_rbdl_pb[tau_idx] += np.absolute(id_rbdl[tau_idx] -
list2np(id_pb[tau_idx]))
error_rbdl_u2c[tau_idx] += np.absolute(id_rbdl[tau_idx] -
u2c2np(id_u2c)[tau_idx])
sum_error_rbdl_pb = 0
sum_error_rbdl_u2c = 0
sum_error_pb_u2c = 0
for err in range(n_joints):
def solveInverseDynamics(self, pos, vel, acc):
return list(p.calculateInverseDynamics(self.robot, pos, vel, acc))
