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
