def test_mass(self):
mass = pin.computeTotalMass(self.model)
self.assertIsNot(mass, np.nan)
mass_check = sum([inertia.mass for inertia in self.model.inertias[1:] ])
self.assertApprox(mass,mass_check)
mass_data = pin.computeTotalMass(self.model,self.data)
self.assertIsNot(mass_data, np.nan)
self.assertApprox(mass,mass_data)
self.assertApprox(mass_data,self.data.mass[0])
data2 = self.model.createData()
pin.centerOfMass(self.model,data2,self.q)
self.assertApprox(mass,data2.mass[0])
def test_mass(self):
mass = pin.computeTotalMass(self.model)
self.assertIsNot(mass, np.nan)
mass_check = sum([inertia.mass for inertia in self.model.inertias[1:] ])
self.assertApprox(mass,mass_check)
mass_data = pin.computeTotalMass(self.model,self.data)
self.assertIsNot(mass_data, np.nan)
self.assertApprox(mass,mass_data)
self.assertApprox(mass_data,self.data.mass[0])
data2 = self.model.createData()
pin.centerOfMass(self.model,data2,self.q)
self.assertApprox(mass,data2.mass[0])
def __init__(self,
robot,
eff_arr,
pinModel=None,
pinData=None,
real_robot=False):
"""
Input:
robot : robot object returned by pinocchio wrapper
eff_arr : end effector name arr
real_robot : bool true if controller running on real robot
"""
if pinModel == None:
self.pin_robot = robot
self.pinModel = self.pin_robot.model
self.pinData = self.pin_robot.data
self.nq = self.pin_robot.nq
self.nv = self.pin_robot.nv
else:
self.pinModel = pinModel
self.pinData = pinData
self.nq = pinModel.nq
self.nv = pinModel.nv
self.robot_mass = pin.computeTotalMass(self.pinModel)
self.eff_arr = eff_arr
def __init__(self, robot, mu, kc, dc, kb, db):
'''
Input:
robot : pinocchio returned robot object
mu : friction co-effecient of the ground
kc : proportional gain on CoM position
kd : derivative gain on CoM position
kb : proportional gain on base orientation
db : derivative gain on base velocity
'''
self.robot = robot
self.robot_mass = pin.computeTotalMass(robot.pin_robot.model)
self.mu = mu # Friction coefficient
self.kc = kc
self.dc = dc
self.kb = kb
self.db = db
self.eff_ids = robot.end_eff_ids
self.qp_penalty_lin = 3 * [
1e6,
]
self.qp_penalty_ang = 3 * [
1e6,
]
self.rotate_vel_error = False
# # Reset the robot to some initial state.
q0 = np.matrix(Solo12Config.initial_configuration).T
dq0 = np.matrix(Solo12Config.initial_velocity).T
robot.reset_state(q0, dq0)
## initialising controllers ############################
eff_arr = ["FL_FOOT", "FR_FOOT", "HL_FOOT", "HR_FOOT"]
robot_ctrl = InverseDynamicsController(robot, eff_arr)
robot_ctrl.set_gains(5.0, 0.1, [1.0, 1.0, 1.0], [1.0, 1.0, 1.0],
[1.0, 1.0, 1.0], [1.0, 1.0, 1.0])
des_q = q0
des_v = dq0
des_a = dq0
des_a[2] = -9.81
F = np.zeros((3 * robot.nb_ee, ))
F[2::3] = pin.computeTotalMass(robot.pin_robot.model) * 9.81 / 4.0
# # Run the simulator for 100 steps
for i in range(4000):
# Step the simulator.
env.step(
sleep=True) # You can sleep here if you want to slow down the replay
# Read the final state and forces after the stepping.
q, dq = robot.get_state()
# passing forces to the impedance controller
tau = robot_ctrl.id_joint_torques(q, dq, des_q, des_v, des_a, F)
# passing torques to the robot
robot.send_joint_command(tau)