def getForce(self, targetPos, vel):
'''
return computed force for each dof by stable pd controller,
you need to write part of this function, please refer to "Stabel Propotional-Derivative Controllers"'s
section 3.1 and 3.3 for details
targetPos : a numpy array of target position(angle) for each dof
vel : target root velocity in facing direction
return : a numpy array of computed torques
'''
pos_now, vel_now = self.getPosandVel()
targetPos = np.array([0]*3 + targetPos.tolist())
targetPos[0] = pos_now[0]
vel = np.array([vel, 0, 0, 0, 0, 0, 0, 0, 0]) # all the dof except for the face direction 's target velocity is 0
pos_part = self.kp.dot(pos_now - targetPos) #position part in pd controller
vel_part = self.kd.dot(vel_now) #velocity part in pd controller
M = p.calculateMassMatrix(self.bipedId, pos_now.tolist()) #mass matrix
M = np.array(M)
# add your code here, please compute the qddot part
# hint: to compute the external force and centrifugal force, you can use p.calculateInverseDynamics()
M = (M + self.kd * self.timeStep)
c = p.calculateInverseDynamics(self.bipedId, pos_now.tolist(), vel_now.tolist(), [0]*9)
c = np.array(c)
b = -pos_part - vel_part -self.kp.dot(vel_now)*self.timeStep - c
qddot = np.linalg.solve(M, b)
tau = -pos_part - vel_part - self.kd.dot(qddot) * self.timeStep-self.kp.dot(vel_now)*self.timeStep + self.kd.dot(vel)
return tau
def getForce(self, targetPos, vel):
pos_now, vel_now = self.getPosandVel()
targetPos = np.array([0]*3 + targetPos.tolist())
targetPos[0] = pos_now[0]
vel = np.array([vel, 0, 0, 0, 0, 0, 0, 0, 0])
pos_part = self.kp.dot(pos_now - targetPos)
vel_part = self.kd.dot(vel_now)
M = p.calculateMassMatrix(self.bipedId, pos_now.tolist())
M = np.array(M)
M = (M + self.kd * self.timeStep)
c = p.calculateInverseDynamics(self.bipedId, pos_now.tolist(), vel_now.tolist(), [0]*9)
c = np.array(c)
b = -pos_part - vel_part -self.kp.dot(vel_now)*self.timeStep - c
qddot = np.linalg.solve(M, b)
tau = -pos_part - vel_part - self.kd.dot(qddot) * self.timeStep-self.kp.dot(vel_now)*self.timeStep + self.kd.dot(vel)
return tau
def get_bullet_mass(joint_pos, joint_vel):
set_pybullet_state(sim, robot_model, num_dofs, joint_pos, joint_vel)
return np.array(
p.calculateMassMatrix(
sim.robot_id, joint_pos, physicsClientId=sim.pc_id
)
)
def step_controller(self):
if self.control_mode is "position":
p.setJointMotorControlArray(bodyUniqueId= self.kuka_uid,
jointIndices= self.joint_indices,
controlMode= p.POSITION_CONTROL,
targetPositions= self.target_q,
targetVelocities= [0] * len(self.joint_names),
forces= [self.max_force] * len(self.joint_names),
physicsClientId = self.client)
elif self.control_mode is "impedance":
states = p.getJointStates(bodyUniqueId = self.kuka_uid,
jointIndices = self.joint_indices,
physicsClientId = self.client)
q = [s[0] for s in states]
q_dot = [s[1] for s in states]
M = p.calculateMassMatrix(bodyUniqueId= self.kuka_uid, objPositions = q, physicsClientId = self.client)
q = np.array(q)
q_des = np.array(self.target_q)
q_dot = np.array(q_dot)
M = np.array(M)
K = np.eye(len(q)) * 80
D = np.eye(len(q)) * 0.1
t = M @ (-K @ (q - q_des) - D @ q_dot)
p.setJointMotorControlArray(bodyUniqueId= self.kuka_uid,
jointIndices= self.joint_indices,
controlMode= p.TORQUE_CONTROL,
targetPositions= [0] * len(self.joint_names),
targetVelocities= [0] * len(self.joint_names),
forces= t,
physicsClientId = self.client)
def test_mass_computation(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"],
)
controlled_joints = robot_model._controlled_joints
for i, joint_idx in enumerate(controlled_joints):
p.resetJointState(bodyUniqueId=robot_id,
jointIndex=joint_idx,
targetValue=test_angles[i],
targetVelocity=test_velocities[i],
physicsClientId=pc_id)
bullet_mass = np.array(
p.calculateMassMatrix(robot_id, test_angles,
physicsClientId=pc_id))
inertia_mat = robot_model.compute_lagrangian_inertia_matrix(
torch.Tensor(test_angles).reshape(1, dof))
assert np.allclose(inertia_mat.detach().squeeze().numpy(),
bullet_mass,
atol=1e-7)
def _calc_mass_matrix(self, q=None):
if q is None:
q = self.q
mass_matrix = pb.calculateMassMatrix(self._pb_sawyer,
q,
physicsClientId=self._clid)
self._mass_matrix = np.array(mass_matrix)[:7, :7]
def mass_matrix(self):
""" compute the system inertia given its joint positions. Uses
rbdl Composite Rigid Body Algorithm.
"""
mass_matrix = pybullet.calculateMassMatrix(
self._arm_id, self.q, physicsClientId=self.physics_id)
return np.array(mass_matrix)[:7, :7]
def calculateDynamicMatrices(self):
joint_pos, joint_vel, _ = self.getJointStates()
n_dof = len(self.controllable_joints)
InertiaMatrix = np.asarray(
p.calculateMassMatrix(self.robot_id, joint_pos))
GravityMatrix = np.asarray(
p.calculateInverseDynamics(self.robot_id, joint_pos, [0.0] * n_dof,
[0.0] * n_dof))
CoriolisMatrix = np.asarray(
p.calculateInverseDynamics(self.robot_id, joint_pos, joint_vel,
[0.0] * n_dof)) - GravityMatrix
return InertiaMatrix, GravityMatrix, CoriolisMatrix
def inertia(self, joint_angles=None):
"""
:param joint_angles: optional parameter, if not None, then returned inertia is evaluated at given joint_angles.
Otherwise, returned inertia tensor is evaluated at current joint angles.
:return: Joint space inertia tensor
"""
if joint_angles is None:
joint_angles = self.angles()
inertia_tensor = np.array(
pb.calculateMassMatrix(self._id, joint_angles.tolist()))
return inertia_tensor
def test_mass_computation(self, request, setup_dict):
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_mass = np.array(
p.calculateMassMatrix(sim.robot_id,
test_case.joint_pos,
physicsClientId=sim.pc_id))
# Differentiable model
inertia_mat = robot_model.compute_lagrangian_inertia_matrix(
torch.Tensor(test_case.joint_pos).reshape(1, num_dofs))
# Compare
assert np.allclose(inertia_mat.detach().squeeze().numpy(),
bullet_mass,
atol=1e-7)
def test_mass_computation(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"],
)
for i in range(7):
p.resetJointState(
bodyUniqueId=robot_id,
jointIndex=i,
targetValue=test_angles[i],
targetVelocity=test_velocities[i],
)
bullet_mass = np.array(p.calculateMassMatrix(robot_id, test_angles))
inertia_mat = robot_model.compute_lagrangian_inertia_matrix(
torch.Tensor(test_angles).reshape(1, 7)
)
assert np.allclose(
inertia_mat.detach().squeeze().numpy(), bullet_mass, atol=1e-7
)
neutral_joint_angles = [0., -0.3135, 0., -2.515, 0., 2.226, 0.87]
#neutral_joint_angles= [0., -0., 0., -0, 0., 0, 0.]
for index, angle in enumerate(neutral_joint_angles):
pb.resetJointState(bodyUniqueId=arm_id,
jointIndex=index,
targetValue=angle,
physicsClientId=physics_id)
pb.stepSimulation(physics_id)
joint_positions = []
for joint_index in range(7):
joint_positions.append(
pb.getJointState(arm_id, joint_index, physics_id)[0])
print("joint positions")
print(joint_positions)
mass_matrix = pb.calculateMassMatrix(bodyUniqueId=arm_id,
objPositions=joint_positions,
physicsClientId=physics_id)
print("Mass Matrix")
tmp = np.array(mass_matrix)
print(tmp)
while 1:
pb.stepSimulation()
# print("hello")
def calculate_mass_matrix(self, *args, **kwargs):
return pb.calculateMassMatrix(*args,
**kwargs,
physicsClientId=self.bullet_cid)
print(model.trav_wave_theta(np.linspace(0, 5, 50), 2, 0.5, 1, 3, 0))
model.set_low_pass_qd(fc=7000)
for index in range(-1, model.num_joints):
p.changeDynamics(model.Id, index, linearDamping=0.0, angularDamping=0.0)
# qd_prev = model.get_joints_speeds_tuple()
# qdd_curr = model.get_joints_acc_nparr(qd_prev=qd_prev)
#print(model.solve_torques_lateral(np.array(list(model.control_indices[0])),qdd_curr))
model.turn_off_crawler()
# for i in range (240):
# p.stepSimulation()
# time.sleep(dt)
#model.turn_off_crawler()
###
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,
def RBD_A(self):
"""
This is the Rigid Body Dynamics A matrix
RBD_A ddq + RBD_B = torque + J.T F
"""
return np.asarray(p.calculateMassMatrix(robot, list(self.state[:GP.QDIMS])))
pybullet_util.draw_link_frame(robot, link_id['ld'], text="distal")
pybullet_util.draw_link_frame(robot, link_id['ee'], text="ee")
# 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)))
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]
rbdl.CompositeRigidBodyAlgorithm(snake_rbdl, q_np, M_rbdl)
kdl.ChainDynParam(snake_chain, gravity_kdl).JntToMass(q_kdl, M_kdl)
M_pb = pb.calculateMassMatrix(snake_pb, q)
M_u2c = M_sym(q)
for row_idx in range(n_joints):
for col_idx in range(n_joints):
error_kdl_u2c[row_idx][col_idx] += np.absolute(M_kdl[row_idx,col_idx] - u2c2np(M_u2c[row_idx, col_idx]))
error_rbdl_u2c[row_idx][col_idx] += np.absolute((M_rbdl[row_idx,col_idx]) - u2c2np(M_u2c[row_idx, col_idx]))
error_pb_u2c[row_idx][col_idx] += np.absolute(list2np(M_pb[row_idx][col_idx]) - u2c2np(M_u2c[row_idx, col_idx]))
error_kdl_rbdl[row_idx][col_idx] += np.absolute((M_rbdl[row_idx,col_idx]) - list2np(M_kdl[row_idx, col_idx]))
error_pb_kdl[row_idx][col_idx] += np.absolute(list2np(M_pb[row_idx][col_idx]) - list2np(M_kdl[row_idx, col_idx]))
error_pb_rbdl[row_idx][col_idx] += np.absolute((M_rbdl[row_idx,col_idx]) - list2np(M_pb[row_idx][col_idx]))
sum_error_kdl_rbdl = 0
sum_error_kdl_u2c = 0
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]
rbdl.CompositeRigidBodyAlgorithm(gantry_rbdl, q_np, M_rbdl)
kdl.ChainDynParam(gantry_chain, gravity_kdl).JntToMass(q_kdl, M_kdl)
M_pb = pb.calculateMassMatrix(gantry_pb, q)
M_u2c = M_sym(q)
for row_idx in range(n_joints):
for col_idx in range(n_joints):
error_kdl_u2c[row_idx][col_idx] += np.absolute(
M_kdl[row_idx, col_idx] - u2c2np(M_u2c[row_idx, col_idx]))
error_rbdl_u2c[row_idx][col_idx] += np.absolute(
(M_rbdl[row_idx, col_idx]) - u2c2np(M_u2c[row_idx, col_idx]))
error_pb_u2c[row_idx][col_idx] += np.absolute(
list2np(M_pb[row_idx][col_idx]) -
u2c2np(M_u2c[row_idx, col_idx]))
error_kdl_rbdl[row_idx][col_idx] += np.absolute(
(M_rbdl[row_idx, col_idx]) - list2np(M_kdl[row_idx, col_idx]))
error_pb_kdl[row_idx][col_idx] += np.absolute(
list2np(M_pb[row_idx][col_idx]) -
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]
rbdl.CompositeRigidBodyAlgorithm(ur5_rbdl, q_np, M_rbdl)
kdl.ChainDynParam(ur5_chain, gravity_kdl).JntToMass(q_kdl, M_kdl)
M_pb = pb.calculateMassMatrix(ur5_pb, q)
M_u2c = M_sym(q)
for row_idx in range(n_joints):
for col_idx in range(n_joints):
error_kdl_u2c[row_idx][col_idx] += np.absolute(
M_kdl[row_idx, col_idx] - u2c2np(M_u2c[row_idx, col_idx]))
error_rbdl_u2c[row_idx][col_idx] += np.absolute(
(M_rbdl[row_idx, col_idx]) - u2c2np(M_u2c[row_idx, col_idx]))
error_pb_u2c[row_idx][col_idx] += np.absolute(
list2np(M_pb[row_idx][col_idx]) -
u2c2np(M_u2c[row_idx, col_idx]))
error_kdl_rbdl[row_idx][col_idx] += np.absolute(
(M_rbdl[row_idx, col_idx]) - list2np(M_kdl[row_idx, col_idx]))
error_pb_kdl[row_idx][col_idx] += np.absolute(
list2np(M_pb[row_idx][col_idx]) -
