def test_trajectory(self):
q_ref = np.ones(5)
traj_euclidian = tsid.TrajectoryEuclidianConstant("traj_eucl", q_ref)
if sys.version_info >= (3, 2):
with self.assertWarns(UserWarning):
traj_euclidian.computeNext().pos()
with self.assertWarns(UserWarning):
traj_euclidian.computeNext().vel()
with self.assertWarns(UserWarning):
traj_euclidian.computeNext().acc()
else:
with warnings.catch_warnings(record=True) as w:
self.assertEqual(len(w), 0)
traj_euclidian.computeNext().pos()
self.assertEqual(len(w), 1)
self.assertEqual(w[-1].category, UserWarning)
traj_euclidian.computeNext().vel()
self.assertEqual(len(w), 2)
self.assertEqual(w[-1].category, UserWarning)
traj_euclidian.computeNext().acc()
self.assertEqual(len(w), 3)
self.assertEqual(w[-1].category, UserWarning)
def control(self, qmes12, vmes12, t):
if (t > 3.0):
self.com_new_ref = np.sin(self.omega * t) + self.com_ref
self.trajCom = tsid.TrajectoryEuclidianConstant(
"traj_com", self.com_new_ref)
self.sampleCom = self.trajCom.computeNext()
self.comTask.setReference(self.sampleCom)
# Resolution of the HQP problem
HQPData = self.invdyn.computeProblemData(t, self.qdes, self.vdes)
self.sol = self.solver.solve(HQPData)
# Torques, accelerations, velocities and configuration computation
tau_ff = self.invdyn.getActuatorForces(self.sol)
self.ades = self.invdyn.getAccelerations(self.sol)
self.vdes += self.ades * dt
self.qdes = pin.integrate(self.model, self.qdes, self.vdes * dt)
# Torque PD controller
P = 50.0
D = 0.2
torques12 = P * (self.qdes[7:] - qmes12[7:]) + D * (
self.vdes[6:] - vmes12[6:]) + tau_ff
torques8 = np.concatenate(
(torques12[1:3], torques12[4:6], torques12[7:9], torques12[10:12]))
# Saturation to limit the maximal torque
t_max = 2.5
tau = np.maximum(np.minimum(torques8, t_max * np.ones((8, 1))),
-t_max * np.ones((8, 1)))
self.error = self.error or (self.sol.status != 0) or (
qmes12[8] < -np.pi / 2) or (qmes12[11] < -np.pi / 2) or (
qmes12[14] < -np.pi / 2) or (qmes12[17] < -np.pi / 2) or (
qmes12[8] > np.pi / 2) or (qmes12[11] > np.pi / 2) or (
qmes12[14] > np.pi / 2) or (qmes12[17] > np.pi / 2)
return tau.flatten()
def log(self, t, solo, k_simu, sequencer, interface):
""" To log various quantities
Not used anymore since logging is done by the Logger object
"""
self.h_ref_feet[k_simu] = interface.mean_feet_z
# Log pos, vel, acc of the flying foot
for i_foot in range(4):
self.f_pos_ref[i_foot, k_simu:(k_simu+1), :] = self.sampleFeet[i_foot].pos()[0:3].transpose()
self.f_vel_ref[i_foot, k_simu:(k_simu+1), :] = self.sampleFeet[i_foot].vel()[0:3].transpose()
self.f_acc_ref[i_foot, k_simu:(k_simu+1), :] = self.sampleFeet[i_foot].acc()[0:3].transpose()
pos = self.robot.framePosition(self.invdyn.data(), self.ID_feet[i_foot])
vel = self.robot.frameVelocityWorldOriented(
self.invdyn.data(), self.ID_feet[i_foot])
acc = self.robot.frameAccelerationWorldOriented(
self.invdyn.data(), self.ID_feet[i_foot])
self.f_pos[i_foot, k_simu:(k_simu+1), :] = interface.o_feet[:,
i_foot] # pos.translation[0:3].transpose()
self.f_vel[i_foot, k_simu:(k_simu+1), :] = vel.vector[0:3].transpose()
self.f_acc[i_foot, k_simu:(k_simu+1), :] = acc.vector[0:3].transpose()
c_f = np.zeros((3, 4))
for i, j in enumerate(np.where(sequencer.S[0, :] == 1)[0]):
c_f[:, j:(j+1)] = self.fc[(3*i):(3*(i+1))]
self.c_forces[:, k_simu, :] = c_f.transpose() # self.fc.reshape((4, 3))
# Log position of the base
pos_trunk = self.robot.framePosition(self.invdyn.data(), self.ID_base)
self.b_pos[k_simu:(k_simu+1), 0:3] = pos_trunk.translation[0:3].transpose()
vel_trunk = self.robot.frameVelocityWorldOriented(self.invdyn.data(), self.ID_base)
self.b_vel[k_simu:(k_simu+1), 0:3] = vel_trunk.vector[0:3].transpose()
# Log position and reference of the CoM
com_ref = self.robot.com(self.invdyn.data())
self.trajCom = tsid.TrajectoryEuclidianConstant("traj_com", com_ref)
sample_com = self.trajCom.computeNext()
self.com_pos_ref[k_simu:(k_simu+1), 0:3] = sample_com.pos().transpose()
self.com_pos[k_simu:(k_simu+1), 0:3] = self.robot.com(self.invdyn.data()).transpose()
def __init__(self, conf, viewer=True):
self.conf = conf
vector = se3.StdVec_StdString()
vector.extend(item for item in conf.path)
self.robot = tsid.RobotWrapper(conf.urdf, vector, False)
robot = self.robot
self.model = model = robot.model()
try:
# q = se3.getNeutralConfiguration(model, conf.srdf, False)
se3.loadReferenceConfigurations(model, conf.srdf, False)
q = model.referenceConfigurations['default']
# q = model.referenceConfigurations["half_sitting"]
except:
q = conf.q0
# q = np.matrix(np.zeros(robot.nv)).T
v = np.matrix(np.zeros(robot.nv)).T
assert model.existFrame(conf.ee_frame_name)
formulation = tsid.InverseDynamicsFormulationAccForce(
"tsid", robot, False)
formulation.computeProblemData(0.0, q, v)
postureTask = tsid.TaskJointPosture("task-posture", robot)
postureTask.setKp(conf.kp_posture * matlib.ones(robot.nv).T)
postureTask.setKd(2.0 * np.sqrt(conf.kp_posture) *
matlib.ones(robot.nv).T)
formulation.addMotionTask(postureTask, conf.w_posture, 1, 0.0)
self.eeTask = tsid.TaskSE3Equality("task-ee", self.robot,
self.conf.ee_frame_name)
self.eeTask.setKp(self.conf.kp_ee * np.matrix(np.ones(6)).T)
self.eeTask.setKd(2.0 * np.sqrt(self.conf.kp_ee) *
np.matrix(np.ones(6)).T)
self.eeTask.setMask(conf.ee_task_mask)
self.eeTask.useLocalFrame(False)
self.EE = model.getFrameId(conf.ee_frame_name)
H_ee_ref = self.robot.framePosition(formulation.data(), self.EE)
self.trajEE = tsid.TrajectorySE3Constant("traj-ee", H_ee_ref)
formulation.addMotionTask(self.eeTask, conf.w_ee, 1, 0.0)
self.tau_max = conf.tau_max_scaling * model.effortLimit
self.tau_min = -self.tau_max
actuationBoundsTask = tsid.TaskActuationBounds("task-actuation-bounds",
robot)
actuationBoundsTask.setBounds(self.tau_min, self.tau_max)
if (conf.w_torque_bounds > 0.0):
formulation.addActuationTask(actuationBoundsTask,
conf.w_torque_bounds, 0, 0.0)
jointBoundsTask = tsid.TaskJointBounds("task-joint-bounds", robot,
conf.dt)
self.v_max = conf.v_max_scaling * model.velocityLimit
self.v_min = -self.v_max
jointBoundsTask.setVelocityBounds(self.v_min, self.v_max)
if (conf.w_joint_bounds > 0.0):
formulation.addMotionTask(jointBoundsTask, conf.w_joint_bounds, 0,
0.0)
trajPosture = tsid.TrajectoryEuclidianConstant("traj_joint", q)
postureTask.setReference(trajPosture.computeNext())
solver = tsid.SolverHQuadProgFast("qp solver")
solver.resize(formulation.nVar, formulation.nEq, formulation.nIn)
self.trajPosture = trajPosture
self.postureTask = postureTask
self.actuationBoundsTask = actuationBoundsTask
self.jointBoundsTask = jointBoundsTask
self.formulation = formulation
self.solver = solver
self.q = q
self.v = v
# for gepetto viewer
if (viewer):
self.robot_display = se3.RobotWrapper.BuildFromURDF(
conf.urdf, [
conf.path,
])
l = subprocess.getstatusoutput(
"ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l")
if int(l[1]) == 0:
os.system('gepetto-gui &')
time.sleep(1)
gepetto.corbaserver.Client()
self.robot_display.initViewer(loadModel=True)
self.robot_display.displayCollisions(False)
self.robot_display.displayVisuals(True)
self.robot_display.display(q)
self.gui = self.robot_display.viewer.gui
self.gui.setCameraTransform(0, conf.CAMERA_TRANSFORM)
import pinocchio as se3
import tsid
import numpy as np
print ""
print "Test Trajectory Euclidian"
print ""
tol = 1e-5
n = 5
q_ref = np.matrix(np.ones(n)).transpose()
zero = np.matrix(np.zeros(n)).transpose()
traj_euclidian = tsid.TrajectoryEuclidianConstant("traj_eucl", q_ref)
assert traj_euclidian.has_trajectory_ended()
assert np.linalg.norm(traj_euclidian.computeNext().pos() - q_ref, 2) < tol
assert np.linalg.norm(traj_euclidian.getSample(0.0).pos() - q_ref, 2) < tol
traj_sample = tsid.TrajectorySample(n)
traj_euclidian.getLastSample(traj_sample)
assert np.linalg.norm(traj_sample.pos() - q_ref, 2) < tol
assert np.linalg.norm(traj_sample.vel() - zero, 2) < tol
assert np.linalg.norm(traj_sample.acc() - zero, 2) < tol
print ""
print "Test Trajectory SE3"
print ""
M_ref = se3.SE3.Identity()
M_vec = np.matrix(np.zeros(12)).transpose()
M_vec[0:3] = M_ref.translation
def __init__(self, N_simulation, k_mpc, n_periods):
self.q_ref = np.array([[0.0, 0.0, 0.2027682, 0.0, 0.0, 0.0, 1.0,
0.0, 0.8, -1.6, 0, 0.8, -1.6,
0, -0.8, 1.6, 0, -0.8, 1.6]]).transpose()
self.qtsid = self.q_ref.copy()
self.vtsid = np.zeros((18, 1))
self.ades = np.zeros((18, 1))
self.error = False
self.verbose = True
# List with the names of all feet frames
self.foot_frames = ['FL_FOOT', 'FR_FOOT', 'HL_FOOT', 'HR_FOOT']
# Constraining the contacts
mu = 0.9 # friction coefficient
fMin = 1.0 # minimum normal force
fMax = 25.0 # maximum normal force
contactNormal = np.matrix([0., 0., 1.]).T # direction of the normal to the contact surface
# Coefficients of the posture task
kp_posture = 10.0 # proportionnal gain of the posture task
w_posture = 1.0 # weight of the posture task
# Coefficients of the contact tasks
kp_contact = 100.0 # proportionnal gain for the contacts
self.w_forceRef = 50.0 # weight of the forces regularization
self.w_reg_f = 50.0
# Coefficients of the foot tracking task
kp_foot = 100.0 # proportionnal gain for the tracking task
self.w_foot = 500.0 # weight of the tracking task
# Arrays to store logs
k_max_loop = N_simulation
self.f_pos = np.zeros((4, k_max_loop, 3))
self.f_vel = np.zeros((4, k_max_loop, 3))
self.f_acc = np.zeros((4, k_max_loop, 3))
self.f_pos_ref = np.zeros((4, k_max_loop, 3))
self.f_vel_ref = np.zeros((4, k_max_loop, 3))
self.f_acc_ref = np.zeros((4, k_max_loop, 3))
self.b_pos = np.zeros((k_max_loop, 6))
self.b_vel = np.zeros((k_max_loop, 6))
self.com_pos = np.zeros((k_max_loop, 3))
self.com_pos_ref = np.zeros((k_max_loop, 3))
self.c_forces = np.zeros((4, k_max_loop, 3))
self.h_ref_feet = np.zeros((k_max_loop, ))
self.goals = np.zeros((3, 4))
self.vgoals = np.zeros((3, 4))
self.agoals = np.zeros((3, 4))
self.mgoals = np.zeros((6, 4))
# Position of the shoulders in local frame
self.shoulders = np.array([[0.19, 0.19, -0.19, -0.19], [0.15005, -0.15005, 0.15005, -0.15005]])
self.footsteps = self.shoulders.copy()
self.memory_contacts = self.shoulders.copy()
# Foot trajectory generator
max_height_feet = 0.05
t_lock_before_touchdown = 0.05
self.ftgs = [ftg.Foot_trajectory_generator(max_height_feet, t_lock_before_touchdown) for i in range(4)]
# Which pair of feet is active (0 for [1, 2] and 1 for [0, 3])
self.pair = -1
# Number of TSID steps for 1 step of the MPC
self.k_mpc = k_mpc
# For update_feet_tasks function
self.dt = 0.001 # [s], time step
self.t1 = 0.14 # [s], duration of swing phase
# Rotation matrix
self.R = np.eye(3)
# Feedforward torques
self.tau_ff = np.zeros((12, 1))
# Torques sent to the robot
self.torques12 = np.zeros((12, 1))
self.tau = np.zeros((12, ))
self.ID_base = None # ID of base link
self.ID_feet = [None] * 4 # ID of feet links
# Footstep planner object
# self.fstep_planner = FootstepPlanner.FootstepPlanner(0.001, 32)
self.vu_m = np.zeros((6, 1))
self.t_stance = 0.16
self.T_gait = 0.32
self.n_periods = n_periods
self.h_ref = 0.235 - 0.01205385
self.t_swing = np.zeros((4, )) # Total duration of current swing phase for each foot
self.contacts_order = [0, 1, 2, 3]
# Parameter to enable/disable hybrid control
self.enable_hybrid_control = False
# Time since the start of the simulation
self.t = 0.0
########################################################################
# Definition of the Model and TSID problem #
########################################################################
# Set the paths where the urdf and srdf file of the robot are registered
modelPath = "/opt/openrobots/share/example-robot-data/robots"
urdf = modelPath + "/solo_description/robots/solo12.urdf"
srdf = modelPath + "/solo_description/srdf/solo.srdf"
vector = pin.StdVec_StdString()
vector.extend(item for item in modelPath)
# Create the robot wrapper from the urdf model (which has no free flyer) and add a free flyer
self.robot = tsid.RobotWrapper(urdf, vector, pin.JointModelFreeFlyer(), False)
self.model = self.robot.model()
# Creation of the Invverse Dynamics HQP problem using the robot
# accelerations (base + joints) and the contact forces
self.invdyn = tsid.InverseDynamicsFormulationAccForce("tsid", self.robot, False)
# Compute the problem data with a solver based on EiQuadProg
t = 0.0
self.invdyn.computeProblemData(t, self.qtsid, self.vtsid)
# Saving IDs for later
self.ID_base = self.model.getFrameId("base_link")
for i, name in enumerate(self.foot_frames):
self.ID_feet[i] = self.model.getFrameId(name)
# Store a frame object to avoid creating one each time
self.pos_foot = self.robot.framePosition(self.invdyn.data(), self.ID_feet[0])
#####################
# LEGS POSTURE TASK #
#####################
# Task definition (creating the task object)
self.postureTask = tsid.TaskJointPosture("task-posture", self.robot)
self.postureTask.setKp(kp_posture * matlib.ones(self.robot.nv-6).T) # Proportional gain
self.postureTask.setKd(2.0 * np.sqrt(kp_posture) * matlib.ones(self.robot.nv-6).T) # Derivative gain
# Add the task to the HQP with weight = w_posture, priority level = 1 (not real constraint)
# and a transition duration = 0.0
self.invdyn.addMotionTask(self.postureTask, w_posture, 1, 0.0)
# TSID Trajectory (creating the trajectory object and linking it to the task)
pin.loadReferenceConfigurations(self.model, srdf, False)
self.trajPosture = tsid.TrajectoryEuclidianConstant("traj_joint", self.q_ref[7:])
self.samplePosture = self.trajPosture.computeNext()
self.postureTask.setReference(self.samplePosture)
############
# CONTACTS #
############
self.contacts = 4*[None] # List to store the rigid contact tasks
for i, name in enumerate(self.foot_frames):
# Contact definition (creating the contact object)
self.contacts[i] = tsid.ContactPoint(name, self.robot, name, contactNormal, mu, fMin, fMax)
self.contacts[i].setKp((kp_contact * matlib.ones(3).T))
self.contacts[i].setKd((2.0 * np.sqrt(kp_contact) * matlib.ones(3).T))
self.contacts[i].useLocalFrame(False)
# Set the contact reference position
H_ref = self.robot.framePosition(self.invdyn.data(), self.ID_feet[i])
H_ref.translation = np.matrix(
[H_ref.translation[0, 0],
H_ref.translation[1, 0],
0.0]).T
self.contacts[i].setReference(H_ref)
# Regularization weight for the force tracking subtask
self.contacts[i].setRegularizationTaskWeightVector(
np.matrix([self.w_reg_f, self.w_reg_f, self.w_reg_f]).T)
# Adding the rigid contact after the reference contact force has been set
self.invdyn.addRigidContact(self.contacts[i], self.w_forceRef)
#######################
# FOOT TRACKING TASKS #
#######################
self.feetTask = 4*[None] # List to store the foot tracking tasks
mask = np.matrix([1.0, 1.0, 1.0, 0.0, 0.0, 0.0]).T
# Task definition (creating the task object)
for i_foot in range(4):
self.feetTask[i_foot] = tsid.TaskSE3Equality(
"foot_track_" + str(i_foot), self.robot, self.foot_frames[i_foot])
self.feetTask[i_foot].setKp(kp_foot * mask)
self.feetTask[i_foot].setKd(2.0 * np.sqrt(kp_foot) * mask)
self.feetTask[i_foot].setMask(mask)
self.feetTask[i_foot].useLocalFrame(False)
# The reference will be set later when the task is enabled
##########
# SOLVER #
##########
# Use EiquadprogFast solver
self.solver = tsid.SolverHQuadProgFast("qp solver")
# Resize the solver to fit the number of variables, equality and inequality constraints
self.solver.resize(self.invdyn.nVar, self.invdyn.nEq, self.invdyn.nIn)
def __init__(self, q0, omega, t):
self.qdes = q0.copy()
self.vdes = np.zeros((8,1))
self.ades = np.zeros((8,1))
self.error = False
kp_posture = 1.0
w_posture = 10.0
########################################################################
# Definition of the Model and TSID problem #
########################################################################
## Set the paths where the urdf and srdf file of the robot are registered
modelPath = "/opt/openrobots/lib/python3.5/site-packages/../../../share/example-robot-data/robots"
urdf = modelPath + "/solo_description/robots/solo.urdf"
srdf = modelPath + "/solo_description/srdf/solo.srdf"
vector = pin.StdVec_StdString()
vector.extend(item for item in modelPath)
## Create the robot wrapper from the urdf model (without the free flyer)
self.robot = tsid.RobotWrapper(urdf, vector, False)
self.model = self.robot.model()
## Creation of the Invverse Dynamics HQP problem using the robot
## accelerations (base + joints) and the contact forces
self.invdyn = tsid.InverseDynamicsFormulationAccForce("tsid", self.robot, False)
# Compute the problem data with a solver based on EiQuadProg
self.invdyn.computeProblemData(t, self.qdes, self.vdes)
# Get the initial data
self.data = self.invdyn.data()
## Task definition
# POSTURE Task
self.postureTask = tsid.TaskJointPosture("task-posture", self.robot)
self.postureTask.setKp(kp_posture * matlib.ones(8).T) # Proportional gain
self.postureTask.setKd(2.0 * np.sqrt(kp_posture) * matlib.ones(8).T) # Derivative gain
# Add the task to the HQP with weight = w_posture, priority level = 0 (as real constraint) and a transition duration = 0.0
self.invdyn.addMotionTask(self.postureTask, w_posture, 0, 0.0)
## TSID Trajectory
pin.loadReferenceConfigurations(self.model, srdf, False)
q_ref = self.model.referenceConfigurations['straight_standing']
trajPosture = tsid.TrajectoryEuclidianConstant("traj_joint", q_ref)
# Set the trajectory as reference for the posture task
samplePosture = trajPosture.computeNext()
self.postureTask.setReference(samplePosture)
## Initialization of the solver
# Use EiquadprogFast solver
self.solver = tsid.SolverHQuadProgFast("qp solver")
# Resize the solver to fit the number of variables, equality and inequality constraints
self.solver.resize(self.invdyn.nVar, self.invdyn.nEq, self.invdyn.nIn)
vector = se3.StdVec_StdString()
vector.extend(item for item in conf.path)
robot = tsid.RobotWrapper(conf.urdf, vector, False)
model = robot.model()
formulation = tsid.InverseDynamicsFormulationAccForce("tsid", robot, False)
q0 = conf.q0
v0 = np.array(np.zeros(robot.nv)).T
formulation.computeProblemData(0.0, q0, v0)
postureTask = tsid.TaskJointPosture("task-posture", robot)
postureTask.setKp(conf.kp_posture * np.ones(robot.nv).T)
postureTask.setKd(2.0 * np.sqrt(conf.kp_posture) * np.ones(robot.nv).T)
formulation.addMotionTask(postureTask, conf.w_posture, 1, 0.0)
trajPosture = tsid.TrajectoryEuclidianConstant("traj_joint", q0)
postureTask.setReference(trajPosture.computeNext())
v_max = conf.v_max_scaling * model.velocityLimit
v_min = -v_max
jointBoundsTask = tsid.TaskJointBounds("task-joint-bounds", robot, conf.dt)
jointBoundsTask.setVelocityBounds(v_min, v_max)
formulation.addMotionTask(jointBoundsTask, conf.w_joint_bounds, 0, 0.0)
solver = tsid.SolverHQuadProgFast("qp solver")
solver.resize(formulation.nVar, formulation.nEq, formulation.nIn)
if (USE_VIEWER):
robot_display = se3.RobotWrapper.BuildFromURDF(conf.urdf, [
conf.path,
])
def generateWholeBodyMotion(cs, fullBody=None, viewer=None):
if not viewer:
print "No viewer linked, cannot display end_effector trajectories."
print "Start TSID ... "
rp = RosPack()
urdf = rp.get_path(
cfg.Robot.packageName
) + '/urdf/' + cfg.Robot.urdfName + cfg.Robot.urdfSuffix + '.urdf'
if cfg.WB_VERBOSE:
print "load robot : ", urdf
#srdf = "package://" + package + '/srdf/' + cfg.Robot.urdfName+cfg.Robot.srdfSuffix + '.srdf'
robot = tsid.RobotWrapper(urdf, pin.StdVec_StdString(),
pin.JointModelFreeFlyer(), False)
if cfg.WB_VERBOSE:
print "robot loaded in tsid."
# FIXME : tsid robotWrapper don't have all the required methods, only pinocchio have them
pinRobot = pin.RobotWrapper.BuildFromURDF(urdf, pin.StdVec_StdString(),
pin.JointModelFreeFlyer(), False)
q = cs.contact_phases[0].reference_configurations[0][:robot.nq].copy()
v = np.matrix(np.zeros(robot.nv)).transpose()
t = 0.0 # time
# init states list with initial state (assume joint velocity is null for t=0)
invdyn = tsid.InverseDynamicsFormulationAccForce("tsid", robot, False)
invdyn.computeProblemData(t, q, v)
data = invdyn.data()
if cfg.EFF_CHECK_COLLISION: # initialise object needed to check the motion
from mlp.utils import check_path
validator = check_path.PathChecker(fullBody, cs, len(q),
cfg.WB_VERBOSE)
if cfg.WB_VERBOSE:
print "initialize tasks : "
comTask = tsid.TaskComEquality("task-com", robot)
comTask.setKp(cfg.kp_com * np.matrix(np.ones(3)).transpose())
comTask.setKd(2.0 * np.sqrt(cfg.kp_com) *
np.matrix(np.ones(3)).transpose())
invdyn.addMotionTask(comTask, cfg.w_com, cfg.level_com, 0.0)
com_ref = robot.com(invdyn.data())
trajCom = tsid.TrajectoryEuclidianConstant("traj_com", com_ref)
amTask = tsid.TaskAMEquality("task-am", robot)
amTask.setKp(cfg.kp_am * np.matrix([1., 1., 0.]).T)
amTask.setKd(2.0 * np.sqrt(cfg.kp_am * np.matrix([1., 1., 0.]).T))
invdyn.addTask(amTask, cfg.w_am, cfg.level_am)
trajAM = tsid.TrajectoryEuclidianConstant("traj_am",
np.matrix(np.zeros(3)).T)
postureTask = tsid.TaskJointPosture("task-joint-posture", robot)
postureTask.setKp(cfg.kp_posture * cfg.gain_vector)
postureTask.setKd(2.0 * np.sqrt(cfg.kp_posture * cfg.gain_vector))
postureTask.mask(cfg.masks_posture)
invdyn.addMotionTask(postureTask, cfg.w_posture, cfg.level_posture, 0.0)
q_ref = cfg.IK_REFERENCE_CONFIG
trajPosture = tsid.TrajectoryEuclidianConstant("traj_joint", q_ref[7:])
orientationRootTask = tsid.TaskSE3Equality("task-orientation-root", robot,
'root_joint')
mask = np.matrix(np.ones(6)).transpose()
mask[0:3] = 0
mask[5] = cfg.YAW_ROT_GAIN
orientationRootTask.setKp(cfg.kp_rootOrientation * mask)
orientationRootTask.setKd(2.0 * np.sqrt(cfg.kp_rootOrientation * mask))
invdyn.addMotionTask(orientationRootTask, cfg.w_rootOrientation,
cfg.level_rootOrientation, 0.0)
root_ref = robot.position(data, robot.model().getJointId('root_joint'))
trajRoot = tsid.TrajectorySE3Constant("traj-root", root_ref)
usedEffectors = []
for eeName in cfg.Robot.dict_limb_joint.values():
if isContactEverActive(cs, eeName):
usedEffectors.append(eeName)
# init effector task objects :
dic_effectors_tasks = createEffectorTasksDic(cs, robot)
effectorTraj = tsid.TrajectorySE3Constant(
"traj-effector", SE3.Identity()
) # trajectory doesn't matter as it's only used to get the correct struct and size
# init empty dic to store effectors trajectories :
dic_effectors_trajs = {}
for eeName in usedEffectors:
dic_effectors_trajs.update({eeName: None})
# add initial contacts :
dic_contacts = {}
for eeName in usedEffectors:
if isContactActive(cs.contact_phases[0], eeName):
contact = createContactForEffector(invdyn, robot,
cs.contact_phases[0], eeName)
dic_contacts.update({eeName: contact})
if cfg.PLOT: # init a dict storing all the reference trajectories used (for plotting)
stored_effectors_ref = {}
for eeName in dic_effectors_tasks:
stored_effectors_ref.update({eeName: []})
solver = tsid.SolverHQuadProg("qp solver")
solver.resize(invdyn.nVar, invdyn.nEq, invdyn.nIn)
# define nested function used in control loop
def storeData(k_t, res, q, v, dv, invdyn, sol):
# store current state
res.q_t[:, k_t] = q
res.dq_t[:, k_t] = v
res.ddq_t[:, k_t] = dv
res.tau_t[:, k_t] = invdyn.getActuatorForces(
sol) # actuator forces, with external forces (contact forces)
#store contact info (force and status)
if cfg.IK_store_contact_forces:
for eeName, contact in dic_contacts.iteritems():
if invdyn.checkContact(contact.name, sol):
res.contact_forces[eeName][:,
k_t] = invdyn.getContactForce(
contact.name, sol)
res.contact_normal_force[
eeName][:, k_t] = contact.getNormalForce(
res.contact_forces[eeName][:, k_t])
res.contact_activity[eeName][:, k_t] = 1
# store centroidal info (real one and reference) :
if cfg.IK_store_centroidal:
pcom, vcom, acom = pinRobot.com(q, v, dv)
res.c_t[:, k_t] = pcom
res.dc_t[:, k_t] = vcom
res.ddc_t[:, k_t] = acom
res.L_t[:, k_t] = pinRobot.centroidalMomentum(q, v).angular
#res.dL_t[:,k_t] = pinRobot.centroidalMomentumVariation(q,v,dv) # FIXME : in robot wrapper, use * instead of .dot() for np matrices
pin.dccrba(pinRobot.model, pinRobot.data, q, v)
res.dL_t[:, k_t] = Force(
pinRobot.data.Ag.dot(dv) + pinRobot.data.dAg.dot(v)).angular
# same for reference data :
res.c_reference[:, k_t] = com_desired
res.dc_reference[:, k_t] = vcom_desired
res.ddc_reference[:, k_t] = acom_desired
res.L_reference[:, k_t] = L_desired
res.dL_reference[:, k_t] = dL_desired
if cfg.IK_store_zmp:
tau = pin.rnea(
pinRobot.model, pinRobot.data, q, v, dv
) # tau without external forces, only used for the 6 first
#res.tau_t[:6,k_t] = tau[:6]
phi0 = pinRobot.data.oMi[1].act(Force(tau[:6]))
res.wrench_t[:, k_t] = phi0.vector
res.zmp_t[:, k_t] = shiftZMPtoFloorAltitude(
cs, res.t_t[k_t], phi0, cfg.EXPORT_OPENHRP)
# same but from the 'reference' values :
Mcom = SE3.Identity()
Mcom.translation = com_desired
Fcom = Force.Zero()
Fcom.linear = cfg.MASS * (acom_desired - cfg.GRAVITY)
Fcom.angular = dL_desired
F0 = Mcom.act(Fcom)
res.wrench_reference[:, k_t] = F0.vector
res.zmp_reference[:, k_t] = shiftZMPtoFloorAltitude(
cs, res.t_t[k_t], F0, cfg.EXPORT_OPENHRP)
if cfg.IK_store_effector:
for eeName in usedEffectors: # real position (not reference)
res.effector_trajectories[eeName][:, k_t] = SE3toVec(
getCurrentEffectorPosition(robot, invdyn.data(), eeName))
res.d_effector_trajectories[eeName][:, k_t] = MotiontoVec(
getCurrentEffectorVelocity(robot, invdyn.data(), eeName))
res.dd_effector_trajectories[eeName][:, k_t] = MotiontoVec(
getCurrentEffectorAcceleration(robot, invdyn.data(),
eeName))
return res
def printIntermediate(v, dv, invdyn, sol):
print "Time %.3f" % (t)
for eeName, contact in dic_contacts.iteritems():
if invdyn.checkContact(contact.name, sol):
f = invdyn.getContactForce(contact.name, sol)
print "\tnormal force %s: %.1f" % (contact.name.ljust(
20, '.'), contact.getNormalForce(f))
print "\ttracking err %s: %.3f" % (comTask.name.ljust(
20, '.'), norm(comTask.position_error, 2))
for eeName, traj in dic_effectors_trajs.iteritems():
if traj:
task = dic_effectors_tasks[eeName]
error = task.position_error
if cfg.Robot.cType == "_3_DOF":
error = error[0:3]
print "\ttracking err %s: %.3f" % (task.name.ljust(
20, '.'), norm(error, 2))
print "\t||v||: %.3f\t ||dv||: %.3f" % (norm(v, 2), norm(dv))
def checkDiverge(res, v, dv):
if norm(dv) > 1e6 or norm(v) > 1e6:
print "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
print "/!\ ABORT : controler unstable at t = " + str(t) + " /!\ "
print "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
raise ValueError("ABORT : controler unstable at t = " + str(t))
if math.isnan(norm(dv)) or math.isnan(norm(v)):
print "!!!!!! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
print "/!\ ABORT : nan at t = " + str(t) + " /!\ "
print "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
raise ValueError("ABORT : controler unstable at t = " + str(t))
def stopHere():
if cfg.WB_ABORT_WHEN_INVALID:
return res.resize(phase_interval[0]), pinRobot
elif cfg.WB_RETURN_INVALID:
return res.resize(k_t), pinRobot
# time check
dt = cfg.IK_dt
if cfg.WB_VERBOSE:
print "dt : ", dt
res = Result(robot.nq, robot.nv, cfg.IK_dt, eeNames=usedEffectors, cs=cs)
N = res.N
last_display = 0
if cfg.WB_VERBOSE:
print "tsid initialized, start control loop"
#raw_input("Enter to start the motion (motion displayed as it's computed, may be slower than real-time)")
time_start = time.time()
# For each phases, create the necessary task and references trajectories :
for pid in range(cs.size()):
if cfg.WB_VERBOSE:
print "## for phase : ", pid
print "t = ", t
phase = cs.contact_phases[pid]
if pid < cs.size() - 1:
phase_next = cs.contact_phases[pid + 1]
else:
phase_next = None
if pid > 0:
phase_prev = cs.contact_phases[pid - 1]
else:
phase_prev = None
t_phase_begin = res.phases_intervals[pid][0] * dt
t_phase_end = res.phases_intervals[pid][-1] * dt
time_interval = [t_phase_begin, t_phase_end]
if cfg.WB_VERBOSE:
print "time_interval ", time_interval
# generate com ref traj from phase :
com_init = np.matrix(np.zeros((9, 1)))
com_init[0:3, 0] = robot.com(invdyn.data())
com_traj = computeCOMRefFromPhase(phase, time_interval)
am_traj = computeAMRefFromPhase(phase, time_interval)
# add root's orientation ref from reference config :
if cfg.USE_PLANNING_ROOT_ORIENTATION:
if phase_next:
root_traj = trajectories.TrajectorySE3LinearInterp(
SE3FromConfig(phase.reference_configurations[0]),
SE3FromConfig(phase_next.reference_configurations[0]),
time_interval)
else:
root_traj = trajectories.TrajectorySE3LinearInterp(
SE3FromConfig(phase.reference_configurations[0]),
SE3FromConfig(phase.reference_configurations[0]),
time_interval)
else:
# orientation such that the torso orientation is the mean between both feet yaw rotations:
placement_init, placement_end = rootOrientationFromFeetPlacement(
phase, phase_next)
root_traj = trajectories.TrajectorySE3LinearInterp(
placement_init, placement_end, time_interval)
# add newly created contacts :
for eeName in usedEffectors:
if phase_prev and not isContactActive(
phase_prev, eeName) and isContactActive(phase, eeName):
invdyn.removeTask(dic_effectors_tasks[eeName].name,
0.0) # remove pin task for this contact
dic_effectors_trajs.update(
{eeName:
None}) # delete reference trajectory for this task
if cfg.WB_VERBOSE:
print "remove se3 task : " + dic_effectors_tasks[
eeName].name
contact = createContactForEffector(invdyn, robot, phase,
eeName)
dic_contacts.update({eeName: contact})
# add se3 tasks for end effector not in contact that will be in contact next phase:
for eeName, task in dic_effectors_tasks.iteritems():
if phase_next and not isContactActive(
phase, eeName) and isContactActive(phase_next, eeName):
if cfg.WB_VERBOSE:
print "add se3 task for " + eeName
invdyn.addMotionTask(task, cfg.w_eff, cfg.level_eff, 0.0)
#create reference trajectory for this task :
placement_init = getCurrentEffectorPosition(
robot, invdyn.data(), eeName
) #FIXME : adjust orientation in case of 3D contact ...
if cfg.Robot.cType == "_3_DOF":
placement_init.rotation = JointPlacementForEffector(
phase, eeName).rotation
placement_end = JointPlacementForEffector(phase_next, eeName)
ref_traj = generateEndEffectorTraj(time_interval,
placement_init,
placement_end, 0)
if cfg.WB_VERBOSE:
print "t interval : ", time_interval
print "placement Init : ", placement_init
print "placement End : ", placement_end
# display all the effector trajectories for this phase
if viewer and cfg.DISPLAY_ALL_FEET_TRAJ:
display_tools.displaySE3Traj(
ref_traj, viewer.client.gui, viewer.sceneName,
eeName + "_traj_" + str(pid),
cfg.Robot.dict_limb_color_traj[eeName], time_interval,
cfg.Robot.dict_offset[eeName])
viewer.client.gui.setVisibility(
eeName + "_traj_" + str(pid), 'ALWAYS_ON_TOP')
dic_effectors_trajs.update({eeName: ref_traj})
# start removing the contact that will be broken in the next phase :
# (This tell the solver that it should start minimzing the contact force on this contact, and ideally get to 0 at the given time)
for eeName, contact in dic_contacts.iteritems():
if phase_next and isContactActive(
phase, eeName) and not isContactActive(phase_next, eeName):
transition_time = t_phase_end - t - dt / 2.
if cfg.WB_VERBOSE:
print "\nTime %.3f Start breaking contact %s. transition time : %.3f\n" % (
t, contact.name, transition_time)
invdyn.removeRigidContact(contact.name, transition_time)
if cfg.WB_STOP_AT_EACH_PHASE:
raw_input('start simulation')
# save values at the beginning of the current phase
q_begin = q.copy()
v_begin = v.copy()
phaseValid = False
swingPhase = False # will be true if an effector move during this phase
iter_for_phase = -1
# iterate until a valid motion for this phase is found (ie. collision free and which respect joint-limits)
while not phaseValid:
if iter_for_phase >= 0:
# reset values to their value at the beginning of the current phase
q = q_begin.copy()
v = v_begin.copy()
iter_for_phase += 1
if cfg.WB_VERBOSE:
print "Start simulation for phase " + str(
pid) + ", try number : " + str(iter_for_phase)
# loop to generate states (q,v,a) for the current contact phase :
if pid == cs.size() - 1: # last state
phase_interval = res.phases_intervals[pid]
else:
phase_interval = res.phases_intervals[pid][:-1]
for k_t in phase_interval:
t = res.t_t[k_t]
# set traj reference for current time :
# com
sampleCom = trajCom.computeNext()
com_desired = com_traj(t)[0]
vcom_desired = com_traj(t)[1]
acom_desired = com_traj(t)[2]
sampleCom.pos(com_desired)
sampleCom.vel(vcom_desired)
sampleCom.acc(acom_desired)
comTask.setReference(sampleCom)
# am
sampleAM = trajAM.computeNext()
L_desired = am_traj(t)[0]
dL_desired = am_traj(t)[1]
sampleAM.pos(L_desired)
sampleAM.vel(dL_desired)
amTask.setReference(sampleAM)
# posture
samplePosture = trajPosture.computeNext()
#print "postural task ref : ",samplePosture.pos()
postureTask.setReference(samplePosture)
# root orientation :
sampleRoot = trajRoot.computeNext()
sampleRoot.pos(SE3toVec(root_traj(t)[0]))
sampleRoot.vel(MotiontoVec(root_traj(t)[1]))
orientationRootTask.setReference(sampleRoot)
quat_waist = Quaternion(root_traj(t)[0].rotation)
res.waist_orientation_reference[:, k_t] = np.matrix(
[quat_waist.x, quat_waist.y, quat_waist.z, quat_waist.w]).T
if cfg.WB_VERBOSE == 2:
print "### references given : ###"
print "com pos : ", sampleCom.pos()
print "com vel : ", sampleCom.vel()
print "com acc : ", sampleCom.acc()
print "AM pos : ", sampleAM.pos()
print "AM vel : ", sampleAM.vel()
print "root pos : ", sampleRoot.pos()
print "root vel : ", sampleRoot.vel()
# end effector (if they exists)
for eeName, traj in dic_effectors_trajs.iteritems():
if traj:
swingPhase = True # there is an effector motion in this phase
sampleEff = effectorTraj.computeNext()
traj_t = traj(t)
sampleEff.pos(SE3toVec(traj_t[0]))
sampleEff.vel(MotiontoVec(traj_t[1]))
dic_effectors_tasks[eeName].setReference(sampleEff)
if cfg.WB_VERBOSE == 2:
print "effector " + str(eeName) + " pos : " + str(
sampleEff.pos())
print "effector " + str(eeName) + " vel : " + str(
sampleEff.vel())
if cfg.IK_store_effector:
res.effector_references[eeName][:, k_t] = SE3toVec(
traj_t[0])
res.d_effector_references[
eeName][:, k_t] = MotiontoVec(traj_t[1])
res.dd_effector_references[
eeName][:, k_t] = MotiontoVec(traj_t[2])
elif cfg.IK_store_effector:
if k_t == 0:
res.effector_references[eeName][:, k_t] = SE3toVec(
getCurrentEffectorPosition(
robot, invdyn.data(), eeName))
else:
res.effector_references[
eeName][:, k_t] = res.effector_references[
eeName][:, k_t - 1]
res.d_effector_references[eeName][:, k_t] = np.matrix(
np.zeros(6)).T
res.dd_effector_references[eeName][:, k_t] = np.matrix(
np.zeros(6)).T
# solve HQP for the current time
HQPData = invdyn.computeProblemData(t, q, v)
if cfg.WB_VERBOSE and t < phase.time_trajectory[0] + dt:
print "final data for phase ", pid
HQPData.print_all()
sol = solver.solve(HQPData)
dv = invdyn.getAccelerations(sol)
res = storeData(k_t, res, q, v, dv, invdyn, sol)
# update state
v_mean = v + 0.5 * dt * dv
v += dt * dv
q = pin.integrate(robot.model(), q, dt * v_mean)
if cfg.WB_VERBOSE == 2:
print "v = ", v
print "dv = ", dv
if cfg.WB_VERBOSE and int(t / dt) % cfg.IK_PRINT_N == 0:
printIntermediate(v, dv, invdyn, sol)
try:
checkDiverge(res, v, dv)
except ValueError:
return stopHere()
# end while t \in phase_t (loop for the current contact phase)
if swingPhase and cfg.EFF_CHECK_COLLISION:
phaseValid, t_invalid = validator.check_motion(
res.q_t[:, phase_interval[0]:k_t])
#if iter_for_phase < 3 :# FIXME : debug only, force limb-rrt
# phaseValid = False
if not phaseValid:
print "Phase " + str(pid) + " not valid at t = " + str(
t_invalid)
if t_invalid <= cfg.EFF_T_PREDEF or t_invalid >= (
(t_phase_end - t_phase_begin) - cfg.EFF_T_PREDEF):
print "Motion is invalid during predefined phases, cannot change this."
return stopHere()
if effectorCanRetry():
print "Try new end effector trajectory."
try:
for eeName, oldTraj in dic_effectors_trajs.iteritems(
):
if oldTraj: # update the traj in the map
placement_init = JointPlacementForEffector(
phase_prev, eeName)
placement_end = JointPlacementForEffector(
phase_next, eeName)
ref_traj = generateEndEffectorTraj(
time_interval, placement_init,
placement_end, iter_for_phase + 1,
res.q_t[:, phase_interval[0]:k_t],
phase_prev, phase, phase_next,
fullBody, eeName, viewer)
dic_effectors_trajs.update(
{eeName: ref_traj})
if viewer and cfg.DISPLAY_ALL_FEET_TRAJ:
display_tools.displaySE3Traj(
ref_traj, viewer.client.gui,
viewer.sceneName,
eeName + "_traj_" + str(pid), cfg.
Robot.dict_limb_color_traj[eeName],
time_interval,
cfg.Robot.dict_offset[eeName])
viewer.client.gui.setVisibility(
eeName + "_traj_" + str(pid),
'ALWAYS_ON_TOP')
except ValueError, e:
print "ERROR in generateEndEffectorTraj :"
print e.message
return stopHere()
else:
print "End effector method choosen do not allow retries, abort here."
return stopHere()
else: # no effector motions, phase always valid (or bypass the check)
phaseValid = True
if cfg.WB_VERBOSE:
print "Phase " + str(pid) + " valid."
if phaseValid:
# display all the effector trajectories for this phase
if viewer and cfg.DISPLAY_FEET_TRAJ and not cfg.DISPLAY_ALL_FEET_TRAJ:
for eeName, ref_traj in dic_effectors_trajs.iteritems():
if ref_traj:
display_tools.displaySE3Traj(
ref_traj, viewer.client.gui, viewer.sceneName,
eeName + "_traj_" + str(pid),
cfg.Robot.dict_limb_color_traj[eeName],
time_interval, cfg.Robot.dict_offset[eeName])
viewer.client.gui.setVisibility(
eeName + "_traj_" + str(pid), 'ALWAYS_ON_TOP')
if cfg.PLOT: # add current ref_traj to the list for plotting
stored_effectors_ref[eeName] += [ref_traj]
print "q:", q.transpose()
se3.centerOfMass(model, data, q)
taskCOM = tsid.TaskComEquality("task-com", robot)
Kp = 100 * np.matrix(np.ones(3)).transpose()
Kd = 20.0 * np.matrix(np.ones(3)).transpose()
taskCOM.setKp(Kp)
taskCOM.setKd(Kd)
assert np.linalg.norm(Kp - taskCOM.Kp, 2) < tol
assert np.linalg.norm(Kd - taskCOM.Kd, 2) < tol
com_ref = data.com[0] + np.matrix(np.ones(3) * 0.02).transpose()
traj = tsid.TrajectoryEuclidianConstant("traj_se3", com_ref)
sample = tsid.TrajectorySample(0)
t = 0.0
dt = 0.001
max_it = 1000
Jpinv = np.matrix(np.zeros((robot.nv, 3)))
error_past = 1e100
v = np.matrix(np.zeros(robot.nv)).transpose()
for i in range(0, max_it):
robot.computeAllTerms(data, q, v)
sample = traj.computeNext()
taskCOM.setReference(sample)
const = taskCOM.compute(t, q, v, data)
def __init__(self, q0, omega, t):
self.omega = omega
self.qdes = q0.copy()
self.vdes = np.zeros((18, 1))
self.ades = np.zeros((18, 1))
self.error = False
kp_posture = 10.0 # proportionnal gain of the posture task
w_posture = 1.0 # weight of the posture task
kp_com = 100.0 # proportionnal gain of the CoM task
w_com = 100.0 # weight of the CoM task
kp_lock = 1.0 # proportionnal gain of the lock task
w_lock = 100.0 # weight of the lock task
# For the contacts
mu = 0.3 # friction coefficient
fMin = 1.0 # minimum normal force
fMax = 100.0 # maximum normal force
w_forceRef = 1e-3 # weight of the forces regularization
kp_contact = 1.0 # proportionnal gain for the contacts
foot_frames = ['HL_FOOT', 'HR_FOOT', 'FL_FOOT',
'FR_FOOT'] # tab with all the foot frames names
contactNormal = np.matrix(
[0., 0., 1.]).T # direction of the normal to the contact surface
########################################################################
# Definition of the Model and TSID problem #
########################################################################
## Set the paths where the urdf and srdf file of the robot are registered
modelPath = "/opt/openrobots/lib/python3.5/site-packages/../../../share/example-robot-data/robots"
urdf = modelPath + "/solo_description/robots/solo12.urdf"
srdf = modelPath + "/solo_description/srdf/solo.srdf"
vector = pin.StdVec_StdString()
vector.extend(item for item in modelPath)
## Create the robot wrapper from the urdf model (without the free flyer)
self.robot = tsid.RobotWrapper(urdf, vector, pin.JointModelFreeFlyer(),
False)
self.model = self.robot.model()
## Creation of the Invverse Dynamics HQP problem using the robot
## accelerations (base + joints) and the contact forces
self.invdyn = tsid.InverseDynamicsFormulationAccForce(
"tsid", self.robot, False)
# Compute the problem data with a solver based on EiQuadProg
self.invdyn.computeProblemData(t, self.qdes, self.vdes)
# Get the initial data
self.data = self.invdyn.data()
## Task definition
# POSTURE Task
self.postureTask = tsid.TaskJointPosture("task-posture", self.robot)
self.postureTask.setKp(
kp_posture * matlib.ones(self.robot.nv - 6).T) # Proportional gain
self.postureTask.setKd(
2.0 * np.sqrt(kp_posture) *
matlib.ones(self.robot.nv - 6).T) # Derivative gain
# Add the task to the HQP with weight = w_posture, priority level = 0 (as real constraint) and a transition duration = 0.0
self.invdyn.addMotionTask(self.postureTask, w_posture, 1, 0.0)
# COM Task
self.comTask = tsid.TaskComEquality("task-com", self.robot)
self.comTask.setKp(
kp_com * matlib.ones(3).T) # Proportional gain of the CoM task
self.comTask.setKd(2.0 * np.sqrt(kp_com) *
matlib.ones(3).T) # Derivative gain
self.invdyn.addMotionTask(
self.comTask, w_com, 1, 0.0
) # Add the task to the HQP with weight = w_com, priority level = 1 (in the cost function) and a transition duration = 0.0
# LOCK Task
self.lockTask = tsid.TaskJointPosture("task-lock-shoulder", self.robot)
self.lockTask.setKp(kp_lock * matlib.ones(self.robot.nv - 6).T)
self.lockTask.setKd(2.0 * np.sqrt(kp_lock) *
matlib.ones(self.robot.nv - 6).T)
mask = np.matrix(np.zeros(
self.robot.nv -
6)) # Add a mask to take into account only the shoulders joints
for i in [0, 3, 6, 9]:
mask[0, i] = 1
self.lockTask.mask(mask.T)
self.invdyn.addMotionTask(
self.lockTask, w_lock, 0,
0.0) # Add the task as real constraint (priority level = 0)
## CONTACTS
self.contacts = 4 * [None]
for i, name in enumerate(foot_frames):
self.contacts[i] = tsid.ContactPoint(name, self.robot, name,
contactNormal, mu, fMin, fMax)
self.contacts[i].setKp(kp_contact * matlib.ones(3).T)
self.contacts[i].setKd(2.0 * np.sqrt(kp_contact) *
matlib.ones(3).T)
H_ref = self.robot.framePosition(self.data,
self.model.getFrameId(name))
self.contacts[i].setReference(H_ref)
self.contacts[i].useLocalFrame(False)
self.invdyn.addRigidContact(self.contacts[i], w_forceRef, 1.0, 1)
## TSID Trajectory
# POSTURE Task
pin.loadReferenceConfigurations(self.model, srdf, False)
self.q_ref = self.model.referenceConfigurations['straight_standing']
self.trajPosture = tsid.TrajectoryEuclidianConstant(
"traj_joint", self.q_ref[7:])
# Set the trajectory as reference for the posture task
self.samplePosture = self.trajPosture.computeNext()
self.postureTask.setReference(self.samplePosture)
# COM Task
self.com_ref = self.data.com[0].copy() # Initial value of the CoM
self.trajCom = tsid.TrajectoryEuclidianConstant(
"traj_com", self.com_ref)
self.sampleCom = self.trajCom.computeNext()
self.comTask.setReference(self.sampleCom)
# LOCK Task
# The mask is enough to set the shoulder acceleration to 0 because 0 is the initial configuration for the shoulders
trajLock = tsid.TrajectoryEuclidianConstant("traj_lock_shoulder",
self.q_ref[7:])
sampleLock = trajLock.computeNext()
self.lockTask.setReference(sampleLock)
## Initialization of the solver
# Use EiquadprogFast solver
self.solver = tsid.SolverHQuadProgFast("qp solver")
# Resize the solver to fit the number of variables, equality and inequality constraints
self.solver.resize(self.invdyn.nVar, self.invdyn.nEq, self.invdyn.nIn)
def __init__(self,
urdf_path,
model_path,
eff_arr,
q0,
v0,
mu=0.6,
fz_max=75):
"""
Input:
urdf_path: path to urdf of robot
model_path: path to model of robot (required by TSID)
eff_arr : end effector name array
q0: initial configuration
v0: initial velocity
tau_max: vector of maximum torques for each joint
mu: coefficient of friction at end-effectors
"""
self.kp_com = 0.0001
self.kp_contact = 5.0
self.kp_posture = 10.0
self.kp_orientation = 0.002
self.w_com = 1.0
self.w_posture = 1e0
self.w_force = 1.0
self.w_orientation = 1e-6
self.contact_transition = 0.1
self.eff_arr = eff_arr
self.curr_cnt_array = np.zeros(len(self.eff_arr))
#self.tau_max = tau_max
#self.tau_min = -1*self.tau_max
self.tsid_robot = tsid.RobotWrapper(urdf_path, [model_path],
pin.JointModelFreeFlyer(), False)
self.nq = self.tsid_robot.nq
self.nv = self.tsid_robot.nv
self.tsid_model = self.tsid_robot.model()
self.invdyn = tsid.InverseDynamicsFormulationAccForce(
"tsid", self.tsid_robot, False)
self.qp_solver = tsid.SolverHQuadProgFast("qp_solver")
#Set up initial solve
self.invdyn.computeProblemData(0, q0.copy(), v0.copy())
self.inv_dyn_data = self.invdyn.data()
self.tsid_robot.computeAllTerms(self.inv_dyn_data, q0, v0)
self.mu = mu
# Add Contact Tasks (setup as equality constraints)
contactNormal = np.array([0., 0., 1.])
self.contact_arr = len(self.eff_arr) * [None]
for i, name in enumerate(self.eff_arr):
self.contact_arr[i] = tsid.ContactPoint(name, self.tsid_robot,
name, contactNormal,
self.mu, 0.01, fz_max)
self.contact_arr[i].setKp(self.kp_contact * np.ones(3))
self.contact_arr[i].setKd(.03 * np.ones(3))
cnt_ref = self.tsid_robot.framePosition(
self.inv_dyn_data,
self.tsid_robot.model().getFrameId(name))
self.contact_arr[i].setReference(cnt_ref)
self.contact_arr[i].useLocalFrame(False)
self.invdyn.addRigidContact(self.contact_arr[i], self.w_force)
# Add posture tasks (in the cost function)
self.postureTask = tsid.TaskJointPosture("posture_task",
self.tsid_robot)
self.postureTask.setKp(self.kp_posture *
np.ones(self.tsid_robot.nv - 6))
self.postureTask.setKd(.05 * np.ones(3))
self.invdyn.addMotionTask(self.postureTask, self.w_posture, 1, 0.0)
#Add CoM task (in the cost function)
# self.comTask = tsid.TaskComEquality("task-com", self.tsid_robot)
# self.comTask.setKp(self.kp_com * np.ones(6))
# self.comTask.setKd(.01 * np.ones(6))
# self.invdyn.addMotionTask(self.comTask, self.w_com, 1, 0.0)
#Add orientation task (in the cost function)
# self.oriTask = tsid.TaskSE3Equality("orientation", self.tsid_robot, "base_link")
# self.oriTask.setKp(self.kp_orientation * np.ones(6))
# self.oriTask.setKd(2.0 * np.sqrt(self.kp_orientation) * np.ones(6))
# mask = np.ones(6)
# mask[:3] = 0
# self.oriTask.setMask(mask)
# self.invdyn.addMotionTask(self.oriTask, self.w_orientation, 1, 0.0)
#Setup trajectories (des_q, des_v, des_a)
# self.com_ref = self.tsid_robot.com(self.inv_dyn_data)
# self.trajCom = tsid.TrajectoryEuclidianConstant("trajectory_com", self.com_ref)
# self.com_reference = self.trajCom.computeNext()
#
self.trajPosture = tsid.TrajectoryEuclidianConstant(
"trajectory_posture", q0[7:])
self.traj_reference = self.trajPosture.computeNext()
self.postureTask.setReference(self.traj_reference)
# self.ori_ref = self.tsid_robot.position(self.inv_dyn_data, self.tsid_robot.model().getJointId("base_link"))
# self.oriTraj = tsid.TrajectorySE3Constant("trajectory_orientation", self.ori_ref)
# self.ori_reference = self.oriTraj.computeNext()
### --- Actuation Bounds --- ###
# Get actuator effort limits (i.e. torque limits) from URDF:
# tau_max = self.actuator_scaling * self.tsid_model.effortLimit
#actuationBounds = tsid.TaskActuationBounds("torque-actuation-bounds", self.tsid_robot)
#actuationBounds.setBounds(self.tau_min, self.tau_max)
#formulation.addActuationTask(actuationBounds, w_torque_bounds, 0, 0.0)
self.qp_solver.resize(self.invdyn.nVar, self.invdyn.nEq,
self.invdyn.nIn)
def __init__(self, conf, dt, urdf, modelPath, srdf):
self.conf = conf
self.dt = dt
vector = pin.StdVec_StdString()
vector.extend(item for item in modelPath)
self.robot = tsid.RobotWrapper(urdf, vector, pin.JointModelFreeFlyer(),
False)
robot = self.robot
self.model = robot.model()
pin.loadReferenceConfigurations(self.model, srdf, False)
try:
self.q0 = conf.q0
q = np.copy(conf.q0)
except:
self.q0 = self.model.referenceConfigurations["half_sitting"]
q = np.copy(self.q0)
self.v0 = v = np.zeros(robot.nv)
assert self.model.existFrame(conf.rf_frame_name)
assert self.model.existFrame(conf.lf_frame_name)
formulation = tsid.InverseDynamicsFormulationAccForce(
"tsid", robot, False)
formulation.computeProblemData(0.0, q, v)
data = formulation.data()
contact_Point = np.ones((3, 4)) * conf.lz
contact_Point[0, :] = [-conf.lxn, -conf.lxn, conf.lxp, conf.lxp]
contact_Point[1, :] = [-conf.lyn, conf.lyp, -conf.lyn, conf.lyp]
contactRF = tsid.Contact6d("contact_rfoot", robot, conf.rf_frame_name,
contact_Point, conf.contact_normal, conf.mu,
conf.fMin, conf.fMax)
contactRF.setKp(conf.kp_contact * np.ones(6))
contactRF.setKd(2.0 * np.sqrt(conf.kp_contact) * np.ones(6))
self.RF = robot.model().getFrameId(conf.rf_frame_name)
H_rf_ref = robot.framePosition(formulation.data(), self.RF)
# print('H RF\n', H_rf_ref.translation)
# modify initial robot configuration so that foot is on the ground (z=0)
q[2] -= H_rf_ref.translation[2] + conf.lz
formulation.computeProblemData(0.0, q, v)
data = formulation.data()
H_rf_ref = robot.framePosition(data, self.RF)
# print('H RF\n', H_rf_ref)
contactRF.setReference(H_rf_ref)
if (conf.w_contact >= 0.0):
formulation.addRigidContact(contactRF, conf.w_forceRef,
conf.w_contact, 1)
else:
formulation.addRigidContact(contactRF, conf.w_forceRef)
contactLF = tsid.Contact6d("contact_lfoot", robot, conf.lf_frame_name,
contact_Point, conf.contact_normal, conf.mu,
conf.fMin, conf.fMax)
contactLF.setKp(conf.kp_contact * np.ones(6))
contactLF.setKd(2.0 * np.sqrt(conf.kp_contact) * np.ones(6))
self.LF = robot.model().getFrameId(conf.lf_frame_name)
H_lf_ref = robot.framePosition(formulation.data(), self.LF)
# print('H LF\n', H_lf_ref)
contactLF.setReference(H_lf_ref)
if (conf.w_contact >= 0.0):
formulation.addRigidContact(contactLF, conf.w_forceRef,
conf.w_contact, 1)
else:
formulation.addRigidContact(contactLF, conf.w_forceRef)
comTask = tsid.TaskComEquality("task-com", robot)
comTask.setKp(conf.kp_com * np.ones(3))
comTask.setKd(2.0 * np.sqrt(conf.kp_com) * np.ones(3))
if (conf.w_com > 0):
formulation.addMotionTask(comTask, conf.w_com, 1, 0.0)
postureTask = tsid.TaskJointPosture("task-posture", robot)
try:
postureTask.setKp(conf.kp_posture)
postureTask.setKd(2.0 * np.sqrt(conf.kp_posture))
except:
postureTask.setKp(conf.kp_posture * np.ones(robot.nv - 6))
postureTask.setKd(2.0 * np.sqrt(conf.kp_posture) *
np.ones(robot.nv - 6))
if (conf.w_posture > 0):
formulation.addMotionTask(postureTask, conf.w_posture, 1, 0.0)
self.leftFootTask = tsid.TaskSE3Equality("task-left-foot", self.robot,
self.conf.lf_frame_name)
self.leftFootTask.setKp(self.conf.kp_foot * np.ones(6))
self.leftFootTask.setKd(2.0 * np.sqrt(self.conf.kp_foot) * np.ones(6))
self.trajLF = tsid.TrajectorySE3Constant("traj-left-foot", H_lf_ref)
if (conf.w_foot > 0):
formulation.addMotionTask(self.leftFootTask, self.conf.w_foot, 1,
0.0)
#
self.rightFootTask = tsid.TaskSE3Equality("task-right-foot",
self.robot,
self.conf.rf_frame_name)
self.rightFootTask.setKp(self.conf.kp_foot * np.ones(6))
self.rightFootTask.setKd(2.0 * np.sqrt(self.conf.kp_foot) * np.ones(6))
self.trajRF = tsid.TrajectorySE3Constant("traj-right-foot", H_rf_ref)
if (conf.w_foot > 0):
formulation.addMotionTask(self.rightFootTask, self.conf.w_foot, 1,
0.0)
self.waistTask = tsid.TaskSE3Equality("task-waist", self.robot,
self.conf.waist_frame_name)
self.waistTask.setKp(self.conf.kp_waist * np.ones(6))
self.waistTask.setKd(2.0 * np.sqrt(self.conf.kp_waist) * np.ones(6))
self.waistTask.setMask(np.array([0, 0, 0, 1, 1, 1.]))
waistID = robot.model().getFrameId(conf.waist_frame_name)
H_waist_ref = robot.framePosition(formulation.data(), waistID)
self.trajWaist = tsid.TrajectorySE3Constant("traj-waist", H_waist_ref)
if (conf.w_waist > 0):
formulation.addMotionTask(self.waistTask, self.conf.w_waist, 1,
0.0)
self.tau_max = conf.tau_max_scaling * robot.model(
).effortLimit[-robot.na:]
self.tau_min = -self.tau_max
actuationBoundsTask = tsid.TaskActuationBounds("task-actuation-bounds",
robot)
actuationBoundsTask.setBounds(self.tau_min, self.tau_max)
if (conf.w_torque_bounds > 0.0):
formulation.addActuationTask(actuationBoundsTask,
conf.w_torque_bounds, 0, 0.0)
jointBoundsTask = tsid.TaskJointBounds("task-joint-bounds", robot, dt)
self.v_max = conf.v_max_scaling * robot.model(
).velocityLimit[-robot.na:]
self.v_min = -self.v_max
jointBoundsTask.setVelocityBounds(self.v_min, self.v_max)
if (conf.w_joint_bounds > 0.0):
formulation.addMotionTask(jointBoundsTask, conf.w_joint_bounds, 0,
0.0)
com_ref = robot.com(formulation.data())
self.trajCom = tsid.TrajectoryEuclidianConstant("traj_com", com_ref)
self.sample_com = self.trajCom.computeNext()
q_ref = q[7:]
self.trajPosture = tsid.TrajectoryEuclidianConstant(
"traj_joint", q_ref)
postureTask.setReference(self.trajPosture.computeNext())
self.sampleLF = self.trajLF.computeNext()
self.sample_LF_pos = self.sampleLF.pos()
self.sample_LF_vel = self.sampleLF.vel()
self.sample_LF_acc = self.sampleLF.acc()
self.sampleRF = self.trajRF.computeNext()
self.sample_RF_pos = self.sampleRF.pos()
self.sample_RF_vel = self.sampleRF.vel()
self.sample_RF_acc = self.sampleRF.acc()
self.solver = tsid.SolverHQuadProgFast("qp solver")
self.solver.resize(formulation.nVar, formulation.nEq, formulation.nIn)
self.comTask = comTask
self.postureTask = postureTask
self.contactRF = contactRF
self.contactLF = contactLF
self.actuationBoundsTask = actuationBoundsTask
self.jointBoundsTask = jointBoundsTask
self.formulation = formulation
self.q = q
self.v = v
self.q0 = np.copy(self.q)
self.contact_LF_active = True
self.contact_RF_active = True
# self.reset()
data = np.load(conf.DATA_FILE_TSID)
# assume dt>dt_ref
r = int(dt / conf.dt_ref)
self.N = int(data['com'].shape[1] / r)
self.contact_phase = data['contact_phase'][::r]
self.com_pos_ref = np.asarray(data['com'])[:, ::r]
self.com_vel_ref = np.asarray(data['dcom'])[:, ::r]
self.com_acc_ref = np.asarray(data['ddcom'])[:, ::r]
self.x_RF_ref = np.asarray(data['x_RF'])[:, ::r]
self.dx_RF_ref = np.asarray(data['dx_RF'])[:, ::r]
self.ddx_RF_ref = np.asarray(data['ddx_RF'])[:, ::r]
self.x_LF_ref = np.asarray(data['x_LF'])[:, ::r]
self.dx_LF_ref = np.asarray(data['dx_LF'])[:, ::r]
self.ddx_LF_ref = np.asarray(data['ddx_LF'])[:, ::r]
self.cop_ref = np.asarray(data['cop'])[:, ::r]
x_rf = self.get_placement_RF().translation
offset = x_rf - self.x_RF_ref[:, 0]
# print("offset", offset)
for i in range(self.N):
self.com_pos_ref[:, i] += offset
self.x_RF_ref[:, i] += offset
self.x_LF_ref[:, i] += offset
def __init__(self, conf, viewer=pin.visualize.MeshcatVisualizer):
self.conf = conf
self.t_limit = False
self.j_limit = True
self.robot = tsid.RobotWrapper(conf.urdf, [conf.path],
pin.JointModelFreeFlyer(), False)
robot = self.robot
self.model = robot.model()
pin.loadReferenceConfigurations(self.model, conf.srdf, False)
self.q0 = q = self.model.referenceConfigurations["half_sitting"]
self.q0[0] = q[0] + 0.003 + 0.6
self.q0[1] = q[1] - 0.001
# self.q0[2] += 0.84
q = self.q0
v = np.zeros(robot.nv)
assert self.model.existFrame(conf.rf_frame_name)
assert self.model.existFrame(conf.lf_frame_name)
formulation = tsid.InverseDynamicsFormulationAccForce(
"tsid", robot, False)
formulation.computeProblemData(0.0, q, v)
data = formulation.data()
contact_Point = np.ones((3, 4)) * conf.lz
contact_Point[0, :] = [-conf.lxn, -conf.lxn, conf.lxp, conf.lxp]
contact_Point[1, :] = [-conf.lyn, conf.lyp, -conf.lyn, conf.lyp]
contactRF = tsid.Contact6d("contact_rfoot", robot, conf.rf_frame_name,
contact_Point, conf.contactNormal, conf.mu,
conf.fMin, conf.fMax)
contactRF.setKp(conf.kp_contact * np.ones(6))
contactRF.setKd(2.0 * np.sqrt(conf.kp_contact) * np.ones(6))
self.RF = robot.model().getFrameId(conf.rf_frame_name)
H_rf_ref = robot.framePosition(data, self.RF)
# modify initial robot configuration so that foot is on the ground (z=0)
q[2] -= H_rf_ref.translation[2] - conf.lz
formulation.computeProblemData(0.0, q, v)
data = formulation.data()
H_rf_ref = robot.framePosition(data, self.RF)
contactRF.setReference(H_rf_ref)
if conf.w_contact >= 0.0:
formulation.addRigidContact(contactRF, conf.w_forceRef,
conf.w_contact, 1)
else:
formulation.addRigidContact(contactRF, conf.w_forceRef)
contactLF = tsid.Contact6d("contact_lfoot", robot, conf.lf_frame_name,
contact_Point, conf.contactNormal, conf.mu,
conf.fMin, conf.fMax)
contactLF.setKp(conf.kp_contact * np.ones(6))
contactLF.setKd(2.0 * np.sqrt(conf.kp_contact) * np.ones(6))
self.LF = robot.model().getFrameId(conf.lf_frame_name)
H_lf_ref = robot.framePosition(data, self.LF)
contactLF.setReference(H_lf_ref)
if conf.w_contact >= 0.0:
formulation.addRigidContact(contactLF, conf.w_forceRef,
conf.w_contact, 1)
else:
formulation.addRigidContact(contactLF, conf.w_forceRef)
comTask = tsid.TaskComEquality("task-com", robot)
comTask.setKp(conf.kp_com * np.ones(3))
comTask.setKd(2.0 * np.sqrt(conf.kp_com) * np.ones(3))
formulation.addMotionTask(comTask, conf.w_com, 1, 0.0)
postureTask = tsid.TaskJointPosture("task-posture", robot)
postureTask.setKp(conf.kp_posture * conf.gain_vector)
postureTask.setKd(2.0 * np.sqrt(conf.kp_posture * conf.gain_vector))
postureTask.mask(conf.masks_posture)
formulation.addMotionTask(postureTask, conf.w_posture, 1, 0.0)
orientationRootTask = tsid.TaskSE3Equality("task-orientation-root",
robot, 'root_joint')
mask = np.ones(6)
mask[0:3] = 0
orientationRootTask.setMask(mask)
orientationRootTask.setKp(conf.kp_rootOrientation * mask)
orientationRootTask.setKd(2.0 *
np.sqrt(conf.kp_rootOrientation * mask))
trajoriRoot = tsid.TrajectorySE3Constant("traj-ori",
pin.SE3().Identity())
formulation.addMotionTask(orientationRootTask, conf.w_rootOrientation,
1, 0.0)
self.amTask = tsid.TaskAMEquality("task-am", robot)
self.amTask.setKp(conf.kp_am * np.array([1., 1., 0.]))
self.amTask.setKd(2.0 * np.sqrt(conf.kp_am * np.array([1., 1., 0.])))
formulation.addMotionTask(self.amTask, conf.w_am, 1, 0.)
self.sampleAM = tsid.TrajectorySample(3)
self.amTask.setReference(self.sampleAM)
self.leftFootTask = tsid.TaskSE3Equality("task-left-foot", self.robot,
self.conf.lf_frame_name)
self.leftFootTask.setKp(self.conf.kp_foot * np.ones(6))
self.leftFootTask.setKd(2.0 * np.sqrt(self.conf.kp_foot) * np.ones(6))
self.trajLF = tsid.TrajectorySE3Constant("traj-left-foot", H_lf_ref)
formulation.addMotionTask(self.leftFootTask, self.conf.w_foot, 1, 0.0)
self.rightFootTask = tsid.TaskSE3Equality("task-right-foot",
self.robot,
self.conf.rf_frame_name)
self.rightFootTask.setKp(self.conf.kp_foot * np.ones(6))
self.rightFootTask.setKd(2.0 * np.sqrt(self.conf.kp_foot) * np.ones(6))
self.trajRF = tsid.TrajectorySE3Constant("traj-right-foot", H_rf_ref)
formulation.addMotionTask(self.rightFootTask, self.conf.w_foot, 1, 0.0)
if self.t_limit:
self.tau_max = conf.tau_max_scaling * robot.model(
).effortLimit[-robot.na:]
self.tau_min = -self.tau_max
actuationBoundsTask = tsid.TaskActuationBounds(
"task-actuation-bounds", robot)
actuationBoundsTask.setBounds(self.tau_min, self.tau_max)
if conf.w_torque_bounds > 0.0:
print("torque_limit")
formulation.addActuationTask(actuationBoundsTask,
conf.w_torque_bounds, 0, 0.0)
if self.j_limit:
jointBoundsTask = tsid.TaskJointBounds("task-joint-bounds", robot,
conf.dt)
self.v_max = conf.v_max_scaling * robot.model(
).velocityLimit[-robot.na:]
self.v_min = -self.v_max
jointBoundsTask.setVelocityBounds(self.v_min, self.v_max)
if conf.w_joint_bounds > 0.0:
print("joint_limit")
formulation.addMotionTask(jointBoundsTask, conf.w_joint_bounds,
0, 0.0)
com_ref = robot.com(data)
self.trajCom = tsid.TrajectoryEuclidianConstant("traj_com", com_ref)
self.sample_com = self.trajCom.computeNext()
q_ref = q[7:]
q_ref[:12] = np.array([
0., 0., -0.4114, 0.8594, -0.448, -0.0017, 0., 0., -0.4114, 0.8594,
-0.448, -0.0017
])
self.trajPosture = tsid.TrajectoryEuclidianConstant(
"traj_joint", q_ref)
postureTask.setReference(self.trajPosture.computeNext())
orientationRootTask.setReference(trajoriRoot.computeNext())
self.sampleLF = self.trajLF.computeNext()
self.sample_LF_pos = self.sampleLF.pos()
self.sample_LF_vel = self.sampleLF.vel()
self.sample_LF_acc = self.sampleLF.acc()
self.sampleRF = self.trajRF.computeNext()
self.sample_RF_pos = self.sampleRF.pos()
self.sample_RF_vel = self.sampleRF.vel()
self.sample_RF_acc = self.sampleRF.acc()
self.solver = tsid.SolverHQuadProgFast("qp solver")
self.solver.resize(formulation.nVar, formulation.nEq, formulation.nIn)
self.comTask = comTask
self.postureTask = postureTask
self.contactRF = contactRF
self.contactLF = contactLF
if self.t_limit:
self.actuationBoundsTask = actuationBoundsTask
if self.j_limit:
self.jointBoundsTask = jointBoundsTask
self.orientationRootTask = orientationRootTask
self.formulation = formulation
self.q = q
self.v = v
self.contact_LF_active = True
self.contact_RF_active = True
if viewer:
self.robot_display = pin.RobotWrapper.BuildFromURDF(
conf.urdf, [conf.path], pin.JointModelFreeFlyer())
if viewer == pin.visualize.GepettoVisualizer:
import gepetto.corbaserver
launched = subprocess.getstatusoutput(
"ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l")
if int(launched[1]) == 0:
os.system('gepetto-gui &')
time.sleep(1)
self.viz = viewer(self.robot_display.model,
self.robot_display.collision_model,
self.robot_display.visual_model)
self.viz.initViewer(loadModel=True)
self.viz.displayCollisions(False)
self.viz.displayVisuals(True)
self.viz.display(q)
self.gui = self.viz.viewer.gui
# self.gui.setCameraTransform(0, conf.CAMERA_TRANSFORM)
# self.gui.addFloor('world/floor')
self.gui.setLightingMode('world/floor', 'OFF')
elif viewer == pin.visualize.MeshcatVisualizer:
self.viz = viewer(self.robot_display.model,
self.robot_display.collision_model,
self.robot_display.visual_model)
self.viz.initViewer(loadModel=True)
self.viz.display(q)
def __init__(self):
########################################################################
# Definition of the Model and TSID problem #
########################################################################
# set the path
path = "/opt/openrobots/share/example-robot-data/robots/solo_description/robots"
urdf = path + "/solo12.urdf"
srdf = "/opt/openrobots/share/example-robot-data/robots/solo_description/srdf/solo.srdf"
vec = pin.StdVec_StdString()
vec.extend(item for item in path)
# get the solo12 wrapper
self.solo12_wrapper = tsid.RobotWrapper(urdf, vec,
pin.JointModelFreeFlyer(),
False)
self.solo12_model = self.solo12_wrapper.model()
pin.loadReferenceConfigurations(self.solo12_model, srdf, False)
# problem formulation
self.formulation = tsid.InverseDynamicsFormulationAccForce(
"tsid", self.solo12_wrapper, False)
self.q0 = self.solo12_model.referenceConfigurations[
"straight_standing"]
self.v0 = np.zeros(self.solo12_model.nv)
self.formulation.computeProblemData(0.0, self.q0, self.v0)
self.data = self.formulation.data()
# get frame ID
self.ID_FL = self.solo12_model.getFrameId("FL_FOOT")
self.ID_FR = self.solo12_model.getFrameId("FR_FOOT")
self.ID_HL = self.solo12_model.getFrameId("HL_FOOT")
self.ID_HR = self.solo12_model.getFrameId("HR_FOOT")
self.ID_BASE = self.solo12_model.getFrameId("base_link")
############
# COM TASK #
############
self.kp_com = 50
self.w_com = 3
self.comTask = tsid.TaskComEquality("task-com", self.solo12_wrapper)
self.comTask.setKp(self.kp_com * np.ones(3).T)
self.comTask.setKd(2 * np.sqrt(self.kp_com) * np.ones(3).T)
self.formulation.addMotionTask(self.comTask, self.w_com, 1, 0.0)
self.com_ref = self.solo12_wrapper.com(self.data)
self.trajCom = tsid.TrajectoryEuclidianConstant(
"traj-com", self.com_ref)
#################
# SE3 BASE TASK #
#################
self.kp_trunk = 50
self.w_trunk = 1
self.trunkTask = tsid.TaskSE3Equality("task-trunk",
self.solo12_wrapper, 'base_link')
mask = np.matrix([1.0, 1.0, 0.0, 1.0, 1.0, 1.0]).T
self.trunkTask.setKp(
np.matrix([
self.kp_trunk, self.kp_trunk, 0.0, self.kp_trunk,
self.kp_trunk, self.kp_trunk
]).T)
self.trunkTask.setKd(
np.matrix([
2.0 * np.sqrt(self.kp_trunk), 2.0 * np.sqrt(self.kp_trunk),
0.0, 2.0 * np.sqrt(self.kp_trunk),
2.0 * np.sqrt(self.kp_trunk), 2.0 * np.sqrt(self.kp_trunk)
]).T)
self.trunkTask.useLocalFrame(False)
self.trunkTask.setMask(mask)
self.formulation.addMotionTask(self.trunkTask, self.w_trunk, 1, 0.0)
self.trunk_ref = self.solo12_wrapper.framePosition(
self.data, self.ID_BASE)
self.trajTrunk = tsid.TrajectorySE3Constant("traj_base_link",
self.trunk_ref)
##################
# CONTACT FORCES #
##################
self.contactNormal = np.array([0, 0, 1])
self.kp_contact = 30
self.mu = 0.5
self.contactFL = tsid.ContactPoint("contactFL", self.solo12_wrapper,
"FL_FOOT", self.contactNormal,
self.mu, 1, 1000)
self.contactFL.setKp(self.kp_contact * np.ones(6).T)
self.contactFL.setKd(2 * np.sqrt(self.kp_contact) * np.ones(6).T)
self.fl_ref = self.solo12_wrapper.framePosition(self.data, self.ID_FL)
self.contactFL.setReference(self.fl_ref)
self.formulation.addRigidContact(self.contactFL, 10)
self.contactFR = tsid.ContactPoint("contactFR", self.solo12_wrapper,
"FR_FOOT", self.contactNormal,
self.mu, 1, 1000)
self.contactFR.setKp(self.kp_contact * np.ones(6).T)
self.contactFR.setKd(2 * np.sqrt(self.kp_contact) * np.ones(6).T)
self.fr_ref = self.solo12_wrapper.framePosition(self.data, self.ID_FR)
self.contactFR.setReference(self.fr_ref)
self.formulation.addRigidContact(self.contactFR, 10)
self.contactHR = tsid.ContactPoint("contactHR", self.solo12_wrapper,
"HR_FOOT", self.contactNormal,
self.mu, 1, 1000)
self.contactHR.setKp(self.kp_contact * np.ones(6).T)
self.contactHR.setKd(2 * np.sqrt(self.kp_contact) * np.ones(6).T)
self.hr_ref = self.solo12_wrapper.framePosition(self.data, self.ID_HR)
self.contactHR.setReference(self.hr_ref)
self.formulation.addRigidContact(self.contactHR, 10)
self.contactHL = tsid.ContactPoint("contactHL", self.solo12_wrapper,
"HL_FOOT", self.contactNormal,
self.mu, 1, 1000)
self.contactHL.setKp(self.kp_contact * np.ones(6).T)
self.contactHL.setKd(2 * np.sqrt(self.kp_contact) * np.ones(6).T)
self.hl_ref = self.solo12_wrapper.framePosition(self.data, self.ID_HL)
self.contactHL.setReference(self.hl_ref)
self.formulation.addRigidContact(self.contactHL, 10)
##########
# SOLVER #
##########
self.solver = tsid.SolverHQuadProgFast("qp solver")
self.solver.resize(self.formulation.nVar, self.formulation.nEq,
self.formulation.nIn)
def __init__(self, conf, viewer=True):
self.conf = conf
self.contact_frame_names = conf.contact_frame_names
print('Robot files:')
print(conf.urdf)
print(conf.srdf)
vector = pin.StdVec_StdString()
vector.extend(item for item in conf.path)
self.robot = tsid.RobotWrapper(conf.urdf, vector,
pin.JointModelFreeFlyer(), False)
robot = self.robot
self.model = robot.model()
pin.loadReferenceConfigurations(self.model, conf.srdf, False)
self.q0 = q = self.model.referenceConfigurations[
conf.reference_config_q_name]
v = np.zeros(robot.nv)
assert (all([
self.model.existFrame(frame_name)
for frame_name in self.contact_frame_names
]))
invdyn = tsid.InverseDynamicsFormulationAccForce('tsid', robot, False)
invdyn.computeProblemData(0.0, q, v)
data = invdyn.data()
robot.computeAllTerms(data, q, v)
#####################################
# define contact and associated tasks
#####################################
self.contact_frame_ids = [
self.model.getFrameId(frame_name)
for frame_name in self.contact_frame_names
]
self.nc = len(self.contact_frame_ids) # contact number
self.contacts = self.nc * [None]
self.tasks_tracking_foot = self.nc * [None]
self.traj_feet = self.nc * [None]
self.sample_feet = self.nc * [None]
mask = np.ones(6)
if not conf.contact6d:
mask[3:] = 0
for i_foot, (frame_name, fid) in enumerate(
zip(self.contact_frame_names, self.contact_frame_ids)):
Hf_ref = robot.framePosition(data, fid)
if conf.contact6d:
# DEFINE FOOT CONTACT POINTS LOCATION WRT LOCAL FOOT FRAMES
# contact_points = np.ones((3,4)) * conf.lz
contact_points = -np.ones((3, 4)) * conf.lz
contact_points[0, :] = [
-conf.lxn, -conf.lxn, conf.lxp, conf.lxp
]
contact_points[1, :] = [
-conf.lyn, conf.lyp, -conf.lyn, conf.lyp
]
# RIGID CONTACT RIGHT FOOT
self.contacts[i_foot] = tsid.Contact6d(
'contact_{}'.format(frame_name), robot, frame_name,
contact_points, conf.contactNormal, conf.mu, conf.fMin,
conf.fMax)
self.contacts[i_foot].setKp(conf.kp_contact * np.ones(6).T)
self.contacts[i_foot].setKd(2.0 * np.sqrt(conf.kp_contact) *
np.ones(6).T)
self.contacts[i_foot].setReference(Hf_ref)
invdyn.addRigidContact(self.contacts[i_foot], conf.w_forceRef)
else:
# RIGID POINT CONTACTS
self.contacts[i_foot] = tsid.ContactPoint(
'contact_{}'.format(frame_name), robot, frame_name,
conf.contactNormal, conf.mu, conf.fMin, conf.fMax)
self.contacts[i_foot].setKp(conf.kp_contact * np.ones(3))
self.contacts[i_foot].setKd(2.0 * np.sqrt(conf.kp_contact) *
np.ones(3))
self.contacts[i_foot].setReference(Hf_ref)
self.contacts[i_foot].useLocalFrame(conf.useLocalFrame)
invdyn.addRigidContact(self.contacts[i_foot], conf.w_forceRef)
# FEET TRACKING TASKS
self.tasks_tracking_foot[i_foot] = tsid.TaskSE3Equality(
'task-foot{}'.format(i_foot), self.robot,
self.contact_frame_names[i_foot])
self.tasks_tracking_foot[i_foot].setKp(conf.kp_foot * mask)
self.tasks_tracking_foot[i_foot].setKd(
2.0 * np.sqrt(conf.kp_foot) * mask)
self.tasks_tracking_foot[i_foot].setMask(mask)
self.tasks_tracking_foot[i_foot].useLocalFrame(False)
invdyn.addMotionTask(self.tasks_tracking_foot[i_foot], conf.w_foot,
conf.priority_foot, 0.0)
self.traj_feet[i_foot] = tsid.TrajectorySE3Constant(
'traj-foot{}'.format(i_foot), Hf_ref)
self.sample_feet[i_foot] = self.traj_feet[i_foot].computeNext()
#############
# Other tasks
#############
# COM TASK
self.comTask = tsid.TaskComEquality('task-com', robot)
self.comTask.setKp(conf.kp_com * np.ones(3))
self.comTask.setKd(2.0 * np.sqrt(conf.kp_com) * np.ones(3))
invdyn.addMotionTask(self.comTask, conf.w_com, conf.priority_com, 1.0)
# POSTURE TASK
self.postureTask = tsid.TaskJointPosture('task-posture', robot)
self.postureTask.setKp(conf.kp_posture * np.ones(robot.nv - 6))
self.postureTask.setKd(2.0 * np.sqrt(conf.kp_posture) *
np.ones(robot.nv - 6))
invdyn.addMotionTask(self.postureTask, conf.w_posture,
conf.priority_posture, 0.0)
# TRUNK Task
self.trunkTask = tsid.TaskSE3Equality("keepTrunk", robot, 'root_joint')
self.trunkTask.setKp(conf.kp_trunk * np.ones(6))
self.trunkTask.setKd(2.0 * np.sqrt(conf.kp_trunk) * np.ones(6))
# Add a Mask to the task which will select the vector dimensions on which the task will act.
# In this case the trunk configuration is a vector 6d (position and orientation -> SE3)
# Here we set a mask = [0 0 0 1 1 1] so the task on the trunk will act on the orientation of the robot
mask = np.ones(6)
mask[:3] = 0.
self.trunkTask.useLocalFrame(False)
self.trunkTask.setMask(mask)
invdyn.addMotionTask(self.trunkTask, conf.w_trunk, conf.priority_trunk,
0.0)
# TORQUE BOUND TASK
# self.tau_max = conf.tau_max_scaling*robot.model().effortLimit[-robot.na:]
# self.tau_min = -self.tau_max
# actuationBoundsTask = tsid.TaskActuationBounds('task-actuation-bounds', robot)
# actuationBoundsTask.setBounds(self.tau_min, self.tau_max)
# if(conf.w_torque_bounds>0.0):
# invdyn.addActuationTask(actuationBoundsTask, conf.w_torque_bounds, conf.priority_torque_bounds, 0.0)
# JOINT BOUND TASK
# jointBoundsTask = tsid.TaskJointBounds('task-joint-bounds', robot, conf.dt)
# self.v_max = conf.v_max_scaling * robot.model().velocityLimit[-robot.na:]
# self.v_min = -self.v_max
# jointBoundsTask.setVelocityBounds(self.v_min, self.v_max)
# if(conf.w_joint_bounds>0.0):
# invdyn.addMotionTask(jointBoundsTask, conf.w_joint_bounds, conf.priority_joint_bounds, 0.0)
##################################
# Init task reference trajectories
##################################
self.sample_feet = self.nc * [None]
self.sample_feet_pos = self.nc * [None]
self.sample_feet_vel = self.nc * [None]
self.sample_feet_acc = self.nc * [None]
for i_foot, traj in enumerate(self.traj_feet):
self.sample_feet[i_foot] = traj.computeNext()
self.sample_feet_pos[i_foot] = self.sample_feet[i_foot].pos()
self.sample_feet_vel[i_foot] = self.sample_feet[i_foot].vel()
self.sample_feet_acc[i_foot] = self.sample_feet[i_foot].acc()
com_ref = robot.com(data)
self.trajCom = tsid.TrajectoryEuclidianConstant('traj_com', com_ref)
self.sample_com = self.trajCom.computeNext()
q_ref = q[7:]
self.trajPosture = tsid.TrajectoryEuclidianConstant(
'traj_joint', q_ref)
self.postureTask.setReference(self.trajPosture.computeNext())
self.trunk_link_id = self.model.getFrameId('base_link')
R_trunk_init = robot.framePosition(data, self.trunk_link_id).rotation
self.trunk_ref = self.robot.framePosition(data, self.trunk_link_id)
self.trajTrunk = tsid.TrajectorySE3Constant("traj_base_link",
self.trunk_ref)
self.sample_trunk = self.trajTrunk.computeNext()
pos_trunk = np.hstack([np.zeros(3), R_trunk_init.flatten()])
self.sample_trunk.pos()
self.sample_trunk.vel(np.zeros(6))
self.sample_trunk.acc(np.zeros(6))
self.trunkTask.setReference(self.sample_trunk)
self.solver = tsid.SolverHQuadProgFast('qp solver')
self.solver.resize(invdyn.nVar, invdyn.nEq, invdyn.nIn)
self.invdyn = invdyn
self.q = q
self.v = v
self.active_contacts = self.nc * [True]
# for gepetto viewer
if (viewer):
self.robot_display = pin.RobotWrapper.BuildFromURDF(
conf.urdf, [
conf.path,
], pin.JointModelFreeFlyer())
l = subprocess.getstatusoutput(
"ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l")
if int(l[1]) == 0:
os.system('gepetto-gui &')
time.sleep(1)
gepetto.corbaserver.Client()
self.robot_display.initViewer(loadModel=True)
self.robot_display.displayCollisions(False)
self.robot_display.displayVisuals(True)
self.robot_display.display(q)
self.gui = self.robot_display.viewer.gui
self.gui.setCameraTransform('python-pinocchio',
conf.CAMERA_TRANSFORM)
self.gui.addFloor('world/floor')
self.gui.setLightingMode('world/floor', 'OFF')
self.gui.addSphere('world/com', conf.SPHERE_RADIUS,
conf.COM_SPHERE_COLOR)
self.gui.addSphere('world/com_ref', conf.REF_SPHERE_RADIUS,
conf.COM_REF_SPHERE_COLOR)
for frame_name in self.contact_frame_names:
self.gui.addSphere('world/{}'.format(frame_name),
conf.SPHERE_RADIUS, conf.F_SPHERE_COLOR)
self.gui.addSphere('world/{}_ref'.format(frame_name),
conf.REF_SPHERE_RADIUS,
conf.F_REF_SPHERE_COLOR)
def __init__(self, conf, viewer=True):
self.conf = conf
vector = se3.StdVec_StdString()
vector.extend(item for item in conf.path)
self.robot = tsid.RobotWrapper(conf.urdf, vector,
se3.JointModelFreeFlyer(), False)
robot = self.robot
self.model = robot.model()
pin.loadReferenceConfigurations(self.model, conf.srdf, False)
self.q0 = q = self.model.referenceConfigurations["half_sitting"]
v = np.zeros(robot.nv)
assert self.model.existFrame(conf.rf_frame_name)
assert self.model.existFrame(conf.lf_frame_name)
formulation = tsid.InverseDynamicsFormulationAccForce(
"tsid", robot, False)
formulation.computeProblemData(0.0, q, v)
data = formulation.data()
contact_Point = np.ones((3, 4)) * conf.lz
contact_Point[0, :] = [-conf.lxn, -conf.lxn, conf.lxp, conf.lxp]
contact_Point[1, :] = [-conf.lyn, conf.lyp, -conf.lyn, conf.lyp]
contactRF = tsid.Contact6d("contact_rfoot", robot, conf.rf_frame_name,
contact_Point, conf.contactNormal, conf.mu,
conf.fMin, conf.fMax)
contactRF.setKp(conf.kp_contact * np.ones(6))
contactRF.setKd(2.0 * np.sqrt(conf.kp_contact) * np.ones(6))
self.RF = robot.model().getJointId(conf.rf_frame_name)
H_rf_ref = robot.position(data, self.RF)
# modify initial robot configuration so that foot is on the ground (z=0)
q[2] -= H_rf_ref.translation[2] - conf.lz
formulation.computeProblemData(0.0, q, v)
data = formulation.data()
H_rf_ref = robot.position(data, self.RF)
contactRF.setReference(H_rf_ref)
formulation.addRigidContact(contactRF, conf.w_forceRef)
contactLF = tsid.Contact6d("contact_lfoot", robot, conf.lf_frame_name,
contact_Point, conf.contactNormal, conf.mu,
conf.fMin, conf.fMax)
contactLF.setKp(conf.kp_contact * np.ones(6))
contactLF.setKd(2.0 * np.sqrt(conf.kp_contact) * np.ones(6))
self.LF = robot.model().getJointId(conf.lf_frame_name)
H_lf_ref = robot.position(data, self.LF)
contactLF.setReference(H_lf_ref)
formulation.addRigidContact(contactLF, conf.w_forceRef)
comTask = tsid.TaskComEquality("task-com", robot)
comTask.setKp(conf.kp_com * np.ones(3))
comTask.setKd(2.0 * np.sqrt(conf.kp_com) * np.ones(3))
formulation.addMotionTask(comTask, conf.w_com, 1, 0.0)
postureTask = tsid.TaskJointPosture("task-posture", robot)
postureTask.setKp(conf.kp_posture * np.ones(robot.nv - 6))
postureTask.setKd(2.0 * np.sqrt(conf.kp_posture) *
np.ones(robot.nv - 6))
formulation.addMotionTask(postureTask, conf.w_posture, 1, 0.0)
self.leftFootTask = tsid.TaskSE3Equality("task-left-foot", self.robot,
self.conf.lf_frame_name)
self.leftFootTask.setKp(self.conf.kp_foot * np.ones(6))
self.leftFootTask.setKd(2.0 * np.sqrt(self.conf.kp_foot) * np.ones(6))
self.trajLF = tsid.TrajectorySE3Constant("traj-left-foot", H_lf_ref)
formulation.addMotionTask(self.leftFootTask, self.conf.w_foot, 1, 0.0)
self.rightFootTask = tsid.TaskSE3Equality("task-right-foot",
self.robot,
self.conf.rf_frame_name)
self.rightFootTask.setKp(self.conf.kp_foot * np.ones(6))
self.rightFootTask.setKd(2.0 * np.sqrt(self.conf.kp_foot) * np.ones(6))
self.trajRF = tsid.TrajectorySE3Constant("traj-right-foot", H_rf_ref)
formulation.addMotionTask(self.rightFootTask, self.conf.w_foot, 1, 0.0)
self.tau_max = conf.tau_max_scaling * robot.model(
).effortLimit[-robot.na:]
self.tau_min = -self.tau_max
actuationBoundsTask = tsid.TaskActuationBounds("task-actuation-bounds",
robot)
actuationBoundsTask.setBounds(self.tau_min, self.tau_max)
if (conf.w_torque_bounds > 0.0):
formulation.addActuationTask(actuationBoundsTask,
conf.w_torque_bounds, 0, 0.0)
jointBoundsTask = tsid.TaskJointBounds("task-joint-bounds", robot,
conf.dt)
self.v_max = conf.v_max_scaling * robot.model(
).velocityLimit[-robot.na:]
self.v_min = -self.v_max
jointBoundsTask.setVelocityBounds(self.v_min, self.v_max)
if (conf.w_joint_bounds > 0.0):
formulation.addMotionTask(jointBoundsTask, conf.w_joint_bounds, 0,
0.0)
com_ref = robot.com(data)
self.trajCom = tsid.TrajectoryEuclidianConstant("traj_com", com_ref)
self.sample_com = self.trajCom.computeNext()
q_ref = q[7:]
self.trajPosture = tsid.TrajectoryEuclidianConstant(
"traj_joint", q_ref)
postureTask.setReference(self.trajPosture.computeNext())
self.sampleLF = self.trajLF.computeNext()
self.sample_LF_pos = self.sampleLF.pos()
self.sample_LF_vel = self.sampleLF.vel()
self.sample_LF_acc = self.sampleLF.acc()
self.sampleRF = self.trajRF.computeNext()
self.sample_RF_pos = self.sampleRF.pos()
self.sample_RF_vel = self.sampleRF.vel()
self.sample_RF_acc = self.sampleRF.acc()
self.solver = tsid.SolverHQuadProgFast("qp solver")
self.solver.resize(formulation.nVar, formulation.nEq, formulation.nIn)
self.comTask = comTask
self.postureTask = postureTask
self.contactRF = contactRF
self.contactLF = contactLF
self.actuationBoundsTask = actuationBoundsTask
self.jointBoundsTask = jointBoundsTask
self.formulation = formulation
self.q = q
self.v = v
self.contact_LF_active = True
self.contact_RF_active = True
# for gepetto viewer
if (viewer):
self.robot_display = se3.RobotWrapper.BuildFromURDF(
conf.urdf, [
conf.path,
], se3.JointModelFreeFlyer())
l = subprocess.getstatusoutput(
"ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l")
if int(l[1]) == 0:
os.system('gepetto-gui &')
time.sleep(1)
gepetto.corbaserver.Client()
self.robot_display.initViewer(loadModel=True)
self.robot_display.displayCollisions(False)
self.robot_display.displayVisuals(True)
self.robot_display.display(q)
self.gui = self.robot_display.viewer.gui
# self.gui.setCameraTransform(0, conf.CAMERA_TRANSFORM)
self.gui.addFloor('world/floor')
self.gui.setLightingMode('world/floor', 'OFF')
postureTask = tsid.TaskJointPosture("task-posture", robot)
postureTask.setKp(kp_posture * np.matrix(np.ones(robot.nv-6)).transpose())
postureTask.setKd(2.0 * np.sqrt(kp_posture) * np.matrix(np.ones(robot.nv-6)).transpose())
invdyn.addMotionTask(postureTask, w_posture, 1, 0.0)
rightFootTask = tsid.TaskSE3Equality("task-right-foot", robot, rf_frame_name)
rightFootTask.setKp(kp_RF * np.matrix(np.ones(6)).transpose())
rightFootTask.setKd(2.0 * np.sqrt(kp_com) * np.matrix(np.ones(6)).transpose())
invdyn.addMotionTask(rightFootTask, w_RF, 1, 0.0)
H_rf_ref.translation += np.matrix([0., 0., DELTA_FOOT_Z]).T
rightFootTraj = tsid.TrajectorySE3Constant("traj-right-foot", H_rf_ref)
com_ref = robot.com(data)
com_ref[1] += DELTA_COM_Y
trajCom = tsid.TrajectoryEuclidianConstant("traj_com", com_ref)
q_ref = q[7:]
trajPosture = tsid.TrajectoryEuclidianConstant("traj_joint", q_ref)
solver = tsid.SolverHQuadProg("qp solver")
solver.resize(invdyn.nVar, invdyn.nEq, invdyn.nIn)
for i in range(0, N_SIMULATION):
time_start = time.time()
if i == REMOVE_CONTACT_N:
print "\nTime %.3f Start breaking contact %s\n"%(t, contactRF.name)
invdyn.removeRigidContact(contactRF.name, CONTACT_TRANSITION_TIME)
sampleCom = trajCom.computeNext()
def __init__(self,
conf,
dt,
robot,
com_offset,
com_frequency,
com_amplitude,
q0=None,
viewer=True):
self.conf = conf
self.dt = dt
self.robot = tsid.RobotWrapper(robot.model, False)
if q0 is None:
q = robot.model.neutralConfiguration
else:
q = np.copy(q0)
# q = se3.getNeutralConfiguration(robot.model(), conf.srdf, False)
# q = robot.model().referenceConfigurations["half_sitting"]
v = np.zeros(robot.nv)
# for gepetto viewer
if (viewer):
# se3.RobotWrapper.BuildFromURDF(conf.urdf, [conf.path, ], se3.JointModelFreeFlyer())
self.robot_display = robot
prompt = subprocess.getstatusoutput(
"ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l")
if int(prompt[1]) == 0:
os.system('gepetto-gui &')
time.sleep(1)
gepetto.corbaserver.Client()
self.robot_display.initViewer(loadModel=True)
self.robot_display.displayCollisions(False)
self.robot_display.displayVisuals(True)
self.robot_display.display(q)
self.gui = self.robot_display.viewer.gui
self.gui.setCameraTransform('gepetto', conf.CAMERA_TRANSFORM)
robot = self.robot
self.model = robot.model()
# assert [robot.model().existFrame(name) for name in conf.contact_frames]
formulation = tsid.InverseDynamicsFormulationAccForce(
"tsid", robot, False)
formulation.computeProblemData(0.0, q, v)
data = formulation.data()
contacts = len(conf.contact_frames) * [None]
self.contact_ids = {}
for i, name in enumerate(conf.contact_frames):
contacts[i] = tsid.ContactPoint(name, robot, name,
conf.contact_normal, conf.mu,
conf.fMin, conf.fMax)
contacts[i].setKp(conf.kp_contact * np.ones(3))
contacts[i].setKd(2.0 * np.sqrt(conf.kp_contact) * np.ones(3))
self.contact_ids[name] = robot.model().getFrameId(name)
H_ref = robot.framePosition(data, robot.model().getFrameId(name))
contacts[i].setReference(H_ref)
contacts[i].useLocalFrame(False)
formulation.addRigidContact(contacts[i], conf.w_forceRef, 1.0, 1)
# modify initial robot configuration so that foot is on the ground (z=0)
# q[2] -= H_rf_ref.translation[2,0] # 0.84 # fix this
# formulation.computeProblemData(0.0, q, v)
# data = formulation.data()
# H_rf_ref = robot.position(data, self.RF)
# contactRF.setReference(H_rf_ref)
# formulation.addRigidContact(contactRF, conf.w_forceRef)
comTask = tsid.TaskComEquality("task-com", robot)
comTask.setKp(conf.kp_com * np.ones(3))
comTask.setKd(2.0 * np.sqrt(conf.kp_com) * np.ones(3))
formulation.addMotionTask(comTask, conf.w_com, 1, 0.0)
postureTask = tsid.TaskJointPosture("task-posture", robot)
postureTask.setKp(conf.kp_posture * np.ones(robot.nv - 6))
postureTask.setKd(2.0 * np.sqrt(conf.kp_posture) *
np.ones(robot.nv - 6))
formulation.addMotionTask(postureTask, conf.w_posture, 1, 0.0)
# self.leftFootTask = tsid.TaskSE3Equality("task-left-foot", self.robot, self.conf.lf_frame_name)
# self.leftFootTask.setKp(self.conf.kp_foot * np.array(np.ones(6)).T)
# self.leftFootTask.setKd(2.0 * np.sqrt(self.conf.kp_foot) * np.array(np.ones(6)).T)
# self.trajLF = tsid.TrajectorySE3Constant("traj-left-foot", H_lf_ref)
#
# self.rightFootTask = tsid.TaskSE3Equality("task-right-foot", self.robot, self.conf.rf_frame_name)
# self.rightFootTask.setKp(self.conf.kp_foot * np.array(np.ones(6)).T)
# self.rightFootTask.setKd(2.0 * np.sqrt(self.conf.kp_foot) * np.array(np.ones(6)).T)
# self.trajRF = tsid.TrajectorySE3Constant("traj-right-foot", H_rf_ref)
#
com_ref = robot.com(data)
trajCom = tsid.TrajectoryEuclidianConstant("traj_com", com_ref)
q_ref = q[7:]
trajPosture = tsid.TrajectoryEuclidianConstant("traj_joint", q_ref)
comTask.setReference(trajCom.computeNext())
postureTask.setReference(trajPosture.computeNext())
solver = tsid.SolverHQuadProgFast("qp solver")
solver.resize(formulation.nVar, formulation.nEq, formulation.nIn)
self.trajCom = trajCom
self.trajPosture = trajPosture
self.comTask = comTask
self.postureTask = postureTask
self.contacts = contacts
self.formulation = formulation
self.solver = solver
self.q = q
self.v = v
self.contacts_active = {}
for name in conf.contact_frames:
self.contacts_active[name] = True
# com_pos = np.empty((3, N_SIMULATION))*nan
# com_vel = np.empty((3, N_SIMULATION))*nan
# com_acc = np.empty((3, N_SIMULATION))*nan
# com_pos_ref = np.empty((3, N_SIMULATION))*nan
# com_vel_ref = np.empty((3, N_SIMULATION))*nan
# com_acc_ref = np.empty((3, N_SIMULATION))*nan
# # acc_des = acc_ref - Kp*pos_err - Kd*vel_err
# com_acc_des = np.empty((3, N_SIMULATION))*nan
self.offset = com_offset + self.robot.com(self.formulation.data())
self.two_pi_f = np.copy(com_frequency)
self.amp = np.copy(com_amplitude)
self.two_pi_f_amp = self.two_pi_f * self.amp
self.two_pi_f_squared_amp = self.two_pi_f * self.two_pi_f_amp
self.sampleCom = self.trajCom.computeNext()
self.reset(q, v, 0.0)