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]) -