def createContactForEffector(invdyn, robot, phase, eeName):
contactNormal = np.matrix(cfg.Robot.dict_normal[eeName]).T
contactNormal = cfg.Robot.dict_offset[
eeName].rotation * contactNormal # apply offset transform
if cfg.Robot.cType == "_3_DOF":
contact = tsid.ContactPoint("contact_" + eeName, robot, eeName,
contactNormal, cfg.MU, cfg.fMin, cfg.fMax)
mask = np.matrix(np.ones(3)).transpose()
contact.useLocalFrame(False)
else:
contact_Points = buildRectangularContactPoints(eeName)
contact = tsid.Contact6d("contact_" + eeName, robot, eeName,
contact_Points, contactNormal, cfg.MU,
cfg.fMin, cfg.fMax)
mask = np.matrix(np.ones(6)).transpose()
contact.setKp(cfg.kp_contact * mask)
contact.setKd(2.0 * np.sqrt(cfg.kp_contact) * mask)
ref = getCurrentEffectorPosition(robot, invdyn.data(), eeName)
contact.setReference(ref)
invdyn.addRigidContact(contact, cfg.w_forceRef)
if cfg.WB_VERBOSE:
print "create contact for effector ", eeName
if cfg.Robot.cType == "_3_DOF":
print "create contact point"
else:
print "create rectangular contact"
print "contact points : \n", contact_Points
print "contact placement : ", ref
print "contact_normal : ", contactNormal
return contact
def add_contact_task(self, kp=10, kd=7, w_forceRef=1e-5, mu=0.5):
fMin = 5.
fMax = 1000.
lxp = 0.14 # foot length in positive x direction
lxn = 0.077 # foot length in negative x direction
lyp = 0.069 # foot length in positive y direction
lyn = 0.069 # foot length in negative y direction
lz = 0.105 # foot sole height with respect to ankle joint
contact_Point = np.matrix(np.ones((3,4)) * lz)
contact_Point[0, :] = [-lxn, -lxn, lxp, lxp]
contact_Point[1, :] = [-lyn, lyp, -lyn, lyp]
contactNormal = np.matrix([0., 0., 1.]).T
pos = self.robot.position(self.invdyn.data(), self.robot.model().getJointId(self.name))
self.contact = tsid.Contact6d(self.name + "_contact",
self.robot, self.name,
contact_Point,
contactNormal,
mu, fMin, fMax, w_forceRef)
self.contact.setKp(kp * np.matrix(np.ones(6)).transpose())
self.contact.setKd(kd * np.matrix(np.ones(6)).transpose())
self.contact.setReference(pos)
self.invdyn.addRigidContact(self.contact)
self.in_contact = True
def createContactForEffector(cfg,
invdyn,
robot,
eeName,
patch,
use_force_reg=True):
"""
Add a contact task in invdyn for the given effector, at it's current placement
:param invdyn:
:param robot:
:param eeName: name of the effector
:param patch: the ContactPatch object to use. Take friction coefficient and placement for the contact from this object
:param use_force_reg: if True, use the cfg.w_forceRef_init otherwise use cfg.w_forceRef_end
:return: the contact task
"""
contactNormal = np.array(cfg.Robot.dict_normal[eeName])
contactNormal = cfg.Robot.dict_offset[
eeName].rotation @ contactNormal # apply offset transform
logger.info("create contact for effector %s", eeName)
logger.info("contact placement : %s", patch.placement)
logger.info("contact_normal : %s", contactNormal)
if patch.contact_model.contact_type == ContactType.CONTACT_POINT:
contact = tsid.ContactPoint("contact_" + eeName, robot, eeName,
contactNormal, patch.friction, cfg.fMin,
cfg.fMax)
mask = np.ones(3)
force_ref = np.zeros(3)
contact.useLocalFrame(False)
logger.info("create contact point")
elif patch.contact_model.contact_type == ContactType.CONTACT_PLANAR:
contact_points = patch.contact_model.contact_points_positions
contact = tsid.Contact6d("contact_" + eeName, robot, eeName,
contact_points, contactNormal, patch.friction,
cfg.fMin, cfg.fMax)
mask = np.ones(6)
force_ref = np.zeros(6)
logger.info("create rectangular contact")
logger.info("contact points : \n %s", contact_points)
else:
raise RuntimeError("Unknown contact type : ",
patch.contact_model.contact_type)
contact.setKp(cfg.kp_contact * mask)
contact.setKd(2.0 * np.sqrt(cfg.kp_contact) * mask)
contact.setReference(patch.placement)
contact.setForceReference(force_ref)
if use_force_reg:
w_forceRef = cfg.w_forceRef_init
else:
w_forceRef = cfg.w_forceRef_end
invdyn.addRigidContact(contact, w_forceRef)
return contact
robot_display.displayCollisions(False)
robot_display.displayVisuals(True)
robot_display.display(q)
assert robot.model().existFrame(rf_frame_name)
assert robot.model().existFrame(lf_frame_name)
t = 0.0 # time
invdyn = tsid.InverseDynamicsFormulationAccForce("tsid", robot, False)
invdyn.computeProblemData(t, q, v)
data = invdyn.data()
contact_Point = np.matrix(np.ones((3,4)) * lz)
contact_Point[0, :] = [-lxn, -lxn, lxp, lxp]
contact_Point[1, :] = [-lyn, lyp, -lyn, lyp]
contactRF =tsid.Contact6d("contact_rfoot", robot, rf_frame_name, contact_Point, contactNormal, mu, fMin, fMax, w_forceRef)
contactRF.setKp(kp_contact * np.matrix(np.ones(6)).transpose())
contactRF.setKd(2.0 * np.sqrt(kp_contact) * np.matrix(np.ones(6)).transpose())
H_rf_ref = robot.position(data, robot.model().getJointId(rf_frame_name))
contactRF.setReference(H_rf_ref)
invdyn.addRigidContact(contactRF)
contactLF =tsid.Contact6d("contact_lfoot", robot, lf_frame_name, contact_Point, contactNormal, mu, fMin, fMax, w_forceRef)
contactLF.setKp(kp_contact * np.matrix(np.ones(6)).transpose())
contactLF.setKd(2.0 * np.sqrt(kp_contact) * np.matrix(np.ones(6)).transpose())
H_lf_ref = robot.position(data, robot.model().getJointId(lf_frame_name))
contactLF.setReference(H_lf_ref)
invdyn.addRigidContact(contactLF)
comTask = tsid.TaskComEquality("task-com", robot)
comTask.setKp(kp_com * np.matrix(np.ones(3)).transpose())
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=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)
lx = 0.07
ly = 0.12
lz = 0.105
mu = 0.3
fMin = 10.0
fMax = 1000.0
frameName = "RAnkleRoll"
contactNormal = np.matrix(np.zeros(3)).transpose()
contactNormal[2] = 1.0
contact_Point = np.matrix(np.ones((3, 4)) * lz)
contact_Point[0, :] = [-lx, -lx, lx, lx]
contact_Point[1, :] = [-ly, ly, -ly, ly]
contact = tsid.Contact6d("contact6d", robot, frameName, contact_Point,
contactNormal, mu, fMin, fMax, 1e-3)
assert contact.n_motion == 6
assert contact.n_force == 12
Kp = np.matrix(np.ones(6)).transpose()
Kd = 2 * Kp
contact.setKp(Kp)
contact.setKd(Kd)
assert np.linalg.norm(contact.Kp - Kp, 2) < tol
assert np.linalg.norm(contact.Kd - Kd, 2) < tol
q = model.neutralConfiguration
v = np.matrix(np.zeros(robot.nv)).transpose()
robot.computeAllTerms(data, q, v)
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')
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
