def com(self, q=None, v=None, a=None, update=True): if (update == False or q is None): return PinocchioRobotWrapper.com(self, q) if a is None: if v is None: return PinocchioRobotWrapper.com(self, q) return PinocchioRobotWrapper.com(self, q, v) return PinocchioRobotWrapper.com(self, q, v, a)
def com(self, q=None, v=None, a=None, update_kinematics=True): if(update_kinematics==False or q is None): return PinocchioRobotWrapper.com(self, q); if q is not None: q_mat = np.asmatrix(q).reshape((self.model.nq,1)) if v is not None: v_mat = np.asmatrix(v).reshape((self.model.nv,1)) if a is None: if v is None: return PinocchioRobotWrapper.com(self, q_mat) return PinocchioRobotWrapper.com(self, q_mat, v_mat) return PinocchioRobotWrapper.com(self, q_mat, v_mat, np.asmatrix(a).reshape((self.model.nv,1)))
import locomote2 from hyq_config import * from pinocchio.robot_wrapper import RobotWrapper import pinocchio as se3 from numpy.linalg import norm, pinv, inv robot = RobotWrapper(urdf_model_path, mesh_dir, root_joint=se3.JointModelFreeFlyer()) model = robot.model data = robot.data com_init = robot.com(robot.q0) com_final = com_init + np.matrix([0.01, 0.0, 0]).transpose() MASS = robot.data.mass[0] robot.initDisplay(loadModel=True) robot.display(q0) # for generating Time Optimization Problem nstep = 5 tp = locomote2.TimeoptProblem(nstep) tp.init_com = com_init tp.final_com = com_final tp.mass = MASS print(com_init) # Adding each contact sequence phase = locomote2.TimeoptPhase() phase.ee_id = locomote2.EndeffectorID.LF