def visualizeCollisionDist(p1, p2, name, color):
gv.addSphere("world/" + name + "/p1", .01, color)
gv.addSphere("world/" + name + "/p2", .01, color)
gv.applyConfiguration("world/" + name + "/p1", p1.tolist() + [1, 0, 0, 0])
gv.applyConfiguration("world/" + name + "/p2", p2.tolist() + [1, 0, 0, 0])
gv.refresh()
### --- display witness as normal patch tangent to capsule
for i in range(2):
gv.addCylinder(
'world/pinocchio/collisions/simple_patch_' + name + '_%d' % i, .01,
.003, color)
direc = (p2 - p1) / norm(p2 - p1)
capsule_radius = 0.0
M1 = pio.SE3(
pio.Quaternion.FromTwoVectors(np.matrix([0, 0, 1]).T,
p1 - p2).matrix(),
p1 + direc * capsule_radius)
M2 = pio.SE3(
pio.Quaternion.FromTwoVectors(np.matrix([0, 0, 1]).T,
p2 - p1).matrix(),
p2 - direc * capsule_radius)
gv.applyConfiguration(
'world/pinocchio/collisions/simple_patch_' + name + '_0',
pio.SE3ToXYZQUATtuple(M1))
gv.applyConfiguration(
'world/pinocchio/collisions/simple_patch_' + name + '_1',
pio.SE3ToXYZQUATtuple(M2))
def displayContact(self, name, force, visibility="ON"):
if visibility =="OFF":
self.gui.setVisibility(self.forceGroup + "/" + name, "OFF")
self.gui.setVisibility(self.frictionGroup + "/" + name, "OFF")
else:
try:
forcePose = self.forcePose(name, force)
# force vector
forceMagnitude = np.linalg.norm(force)
if forceMagnitude > 25.:
forceMagnitude = 25.
forceName = self.forceGroup + "/" + name
self.gui.setVector3Property(forceName, "Scale", [1. * forceMagnitude, 1.5, 1.5])
self.gui.applyConfiguration(forceName, pin.SE3ToXYZQUATtuple(forcePose))
self.gui.setVisibility(forceName, "ALWAYS_ON_TOP")
# friction cone
normalNorm = force.dot(self.z_axis)
if normalNorm > 7.5:
normalNorm = 7.5
conePose = self.conePose(forcePose, normalNorm)
coneName = self.frictionGroup + "/" + name
self.gui.setVector3Property(coneName, "Scale", [normalNorm, normalNorm, normalNorm])
self.gui.applyConfiguration(coneName, pin.SE3ToXYZQUATtuple(conePose))
self.gui.setVisibility(coneName, "ON")
except:
raise BaseException("failed to display contact visuals")
def callback(self, q):
if self.viz is None:
return
vizutils.applyViewerConfiguration(self.viz, 'world/ball',
pin.SE3ToXYZQUATtuple(M))
vizutils.applyViewerConfiguration(self.viz, 'world/box',
pin.SE3ToXYZQUATtuple(self.Mtarget))
self.viz.display(q)
time.sleep(1e-2)
def callback(self, q):
print(1)
q = np.matrix(q).T
pin.forwardKinematics(robot.model, robot.data, q)
M = pin.updateFramePlacement(robot.model, robot.data, self.frameIndex)
gv.applyConfiguration('world/blue', pin.SE3ToXYZQUATtuple(M))
gv.applyConfiguration('world/box', pin.SE3ToXYZQUATtuple(self.Mtarget))
robot.display(q)
time.sleep(1e-2)
def visualizeTorques(gv, rmodel, rdata, tau_q, init=False):
solo12 = (len(tau_q) == 12)
for k in range(len(tau_q)):
jointFrame = rdata.oMi[k+2]
#jointFrame = rdata.oMi[k]
name = 'world/pinocchio/collisions/torque_' + str(k)
color = [0,0,1,1]
additional_transl = np.array([0,0,0.0])
if solo12:
if(k%3==0):
direc = [1,0,0] if tau_q[k]>0 else [-1,0,0]
additional_transl = np.array([0,0,0.05])
else:
direc = [0,1,0] if tau_q[k]>0 else [0,-1,0]
else:
direc = [0,1,0] if tau_q[k]>0 else [0,-1,0]
additional_transl.resize(3,1)
orientation = pin.SE3(pin.Quaternion.FromTwoVectors(np.matrix([1,0,0]).T,np.matrix(direc).T).matrix(),jointFrame.translation + additional_transl)
if(init):
gv.addArrow(name, 0.003, 1, color)
gv.resizeArrow(name, 0.003, np.abs(tau_q[k]))
gv.applyConfiguration(name, pin.SE3ToXYZQUATtuple(orientation))
def compute(m):
tq_vec = pin.SE3ToXYZQUAT(m)
tq_tup = pin.SE3ToXYZQUATtuple(m)
mm_vec = pin.XYZQUATToSE3(tq_vec)
mm_tup = pin.XYZQUATToSE3(tq_tup)
mm_lis = pin.XYZQUATToSE3(list(tq_tup))
return tq_vec, tq_tup, mm_vec, mm_tup, mm_lis
def display(self, q, force):
"""Display the robot at configuration q in the viewer by placing all the bodies."""
gui = self.gui
pin.framesForwardKinematics(self.model,self.data,q)
if self.display_visuals:
pin.updateGeometryPlacements(self.model, self.data, self.visual_model, self.visual_data)
gui.applyConfigurations (
[ self.getViewerNodeName(visual,pin.GeometryType.VISUAL) for visual in self.visual_model.geometryObjects ],
[ pin.SE3ToXYZQUATtuple(self.visual_data.oMg[self.visual_model.getGeometryId(visual.name)]) for visual in self.visual_model.geometryObjects ]
)
for contactIndex, contactName in enumerate(self.contactNames):
forceVector = np.zeros(3)
if(self.type=="Biped"):
for i in range(4):
forceVector += force[:,4*contactIndex+i]
elif(self.type=="Quadruped"):
forceVector =force[:,contactIndex]
forceNorm = np.linalg.norm(forceVector)
if forceNorm<=1.e-3:
forceVisiblity = "OFF"
else:
forceVisiblity = "ON"
self.displayContact(contactName, forceVector, forceVisiblity)
gui.refresh()
def display(self, q):
RobotWrapper.display(self, q)
M1 = self.data.oMi[1]
self.viewer.gui.applyConfiguration('world/mobilebasis',
pin.SE3ToXYZQUATtuple(M1))
self.viewer.gui.applyConfiguration('world/mobilewheel1',
pin.SE3ToXYZQUATtuple(M1))
self.viewer.gui.applyConfiguration('world/mobilewheel2',
pin.SE3ToXYZQUATtuple(M1))
self.viewer.gui.refresh()
pin.updateFramePlacements(self.model, self.data)
self.viewer.gui.applyConfiguration(
'world/framebasis', pin.SE3ToXYZQUATtuple(self.data.oMf[-2]))
self.viewer.gui.applyConfiguration(
'world/frametool', pin.SE3ToXYZQUATtuple(self.data.oMf[-1]))
self.viewer.gui.refresh()
def loadInGui(self, v):
gui = v.client.gui
_add_fov_to_gui(gui, "field_of_view", self.fov,
color = [0.1, 0.1, 0.9, 0.2],
group = "robot/tiago/head_2_link")
idl = self.model.getFrameId("head_2_link")
idc = self.model.getFrameId("xtion_rgb_optical_frame")
assert self.model.frames[idl].parent == self.model.frames[idc].parent
gui.applyConfiguration("field_of_view", pinocchio.SE3ToXYZQUATtuple(
self.model.frames[idl].placement.inverse() * self.model.frames[idc].placement))
gui.refresh()
def callback(self, q):
if self.viz is None:
return
pin.framesForwardKinematics(self.rmodel, self.rdata, q)
M = self.rdata.oMf[self.frameIndex]
vizutils.applyViewerConfiguration(self.viz, 'world/ball',
pin.SE3ToXYZQUATtuple(M))
vizutils.applyViewerConfiguration(self.viz, 'world/box',
self.ptarget.tolist() + [1, 0, 0, 0])
self.viz.display(q)
time.sleep(1e-2)
def visualizeCollisionDist(gv, p1, p2, name, color, init=False):
### --- display witness as normal patch tangent to capsule
if(init):
for i in range(2):
gv.addCylinder('world/pinocchio/collisions/simple_patch_' + name + '_%d'%i, .01, .003, color)
gv.addLine('world/pinocchio/collisions/line_' + name, p1.tolist(), p2.tolist(), color)
direc = (p2-p1)/np.linalg.norm(p2-p1)
M1 = pin.SE3(pin.Quaternion.FromTwoVectors(np.matrix([0,0,1]).T,p1-p2).matrix(),p1)
M2 = pin.SE3(pin.Quaternion.FromTwoVectors(np.matrix([0,0,1]).T,p2-p1).matrix(),p2)
gv.applyConfiguration('world/pinocchio/collisions/simple_patch_' + name + '_0',pin.SE3ToXYZQUATtuple(M1))
gv.applyConfiguration('world/pinocchio/collisions/simple_patch_' + name + '_1',pin.SE3ToXYZQUATtuple(M2))
gv.setLineExtremalPoints('world/pinocchio/collisions/line_' + name, p1.tolist(), p2.tolist())
gv.setColor('world/pinocchio/collisions/simple_patch_' + name + '_0', color)
gv.setColor('world/pinocchio/collisions/simple_patch_' + name + '_1', color)
gv.setColor('world/pinocchio/collisions/line_' + name, color)
gv.refresh()
def display(self, xs, fs=[], ps=[], dts=[], factor=1.):
numpy_conversion = False
if libcrocoddyl_pywrap.getNumpyType() == np.matrix:
numpy_conversion = True
libcrocoddyl_pywrap.switchToNumpyMatrix()
warnings.warn(
"Numpy matrix supports will be removed in future release",
DeprecationWarning,
stacklevel=2)
if ps:
for key, p in ps.items():
self.robot.viewer.gui.setCurvePoints(
self.frameTrajGroup + "/" + key, p)
if not dts:
dts = [0.] * len(xs)
S = 1 if self.rate <= 0 else max(len(xs) / self.rate, 1)
for i, x in enumerate(xs):
if not i % S:
if fs:
self.activeContacts = {
k: False
for k, c in self.activeContacts.items()
}
for f in fs[i]:
key = f["key"]
pose = f["oMf"]
wrench = f["f"]
# Display the contact forces
R = rotationMatrixFromTwoVectors(
self.x_axis, wrench.linear)
forcePose = pinocchio.SE3ToXYZQUATtuple(
pinocchio.SE3(R, pose.translation))
forceMagnitud = np.linalg.norm(
wrench.linear) / self.totalWeight
forceName = self.forceGroup + "/" + key
self.robot.viewer.gui.setVector3Property(
forceName, "Scale", [1. * forceMagnitud, 1., 1.])
self.robot.viewer.gui.applyConfiguration(
forceName, forcePose)
self.robot.viewer.gui.setVisibility(forceName, "ON")
# Display the friction cones
position = pose
position.rotation = rotationMatrixFromTwoVectors(
self.z_axis, f["nsurf"])
frictionName = self.frictionGroup + "/" + key
self._setConeMu(key, f["mu"])
self.robot.viewer.gui.applyConfiguration(
frictionName,
list(
np.array(pinocchio.SE3ToXYZQUAT(
position)).squeeze()))
self.robot.viewer.gui.setVisibility(frictionName, "ON")
self.activeContacts[key] = True
for key, c in self.activeContacts.items():
if c == False:
self.robot.viewer.gui.setVisibility(
self.forceGroup + "/" + key, "OFF")
self.robot.viewer.gui.setVisibility(
self.frictionGroup + "/" + key, "OFF")
self.robot.display(x[:self.robot.nq])
time.sleep(dts[i] * factor)
if numpy_conversion:
numpy_conversion = False
libcrocoddyl_pywrap.switchToNumpyMatrix()
J_com = tsid.comTask.compute(t, q, v, data).matrix
if i % conf.DISPLAY_N == 0:
tsid.display(q)
x_com = tsid.robot.com(tsid.formulation.data())
x_com_ref = tsid.trajCom.getSample(t).pos()
H_lf = tsid.robot.framePosition(tsid.formulation.data(), tsid.LF)
H_rf = tsid.robot.framePosition(tsid.formulation.data(), tsid.RF)
x_lf_ref = tsid.trajLF.getSample(t).pos()[:3]
x_rf_ref = tsid.trajRF.getSample(t).pos()[:3]
vizutils.applyViewerConfiguration(tsid.viz, 'world/com',
x_com.tolist() + [0, 0, 0, 1.])
vizutils.applyViewerConfiguration(tsid.viz, 'world/com_ref',
x_com_ref.tolist() + [0, 0, 0, 1.])
vizutils.applyViewerConfiguration(tsid.viz, 'world/rf',
pin.SE3ToXYZQUATtuple(H_rf))
vizutils.applyViewerConfiguration(tsid.viz, 'world/lf',
pin.SE3ToXYZQUATtuple(H_lf))
vizutils.applyViewerConfiguration(tsid.viz, 'world/rf_ref',
x_rf_ref.tolist() + [0, 0, 0, 1.])
vizutils.applyViewerConfiguration(tsid.viz, 'world/lf_ref',
x_lf_ref.tolist() + [0, 0, 0, 1.])
if i % 100 == 0 and t > 4 and t <= Walk_phases.getFinalTime(
len(Walk_phases.p) - 1) + time_offset * conf.dt:
print('current com', tsid.robot.com(tsid.formulation.data()))
print('desired com', CurveSet.com_traj[i - time_offset])
if tsid.formulation.checkContact(tsid.contactRF.name, sol):
f = tsid.formulation.getContactForce(tsid.contactRF.name, sol)
print("\tnormal force %s: %.1f" % (tsid.contactRF.name.ljust(
20, '.'), tsid.contactRF.getNormalForce(f)))
J_RF = tsid.contactRF.computeMotionTask(0.0, q, v, data).matrix
else:
J_RF = np.zeros((0, tsid.model.nv))
J = np.vstack((J_LF, J_RF))
J_com = tsid.comTask.compute(t, q, v, data).matrix
if i % conf.DISPLAY_N == 0:
tsid.display(q)
x_com = tsid.robot.com(tsid.formulation.data())
x_com_ref = tsid.trajCom.getSample(t).pos()
H_lf = tsid.robot.framePosition(tsid.formulation.data(), tsid.LF)
H_rf = tsid.robot.framePosition(tsid.formulation.data(), tsid.RF)
x_lf_ref = tsid.trajLF.getSample(t).pos()[:3]
x_rf_ref = tsid.trajRF.getSample(t).pos()[:3]
vizutils.applyViewerConfiguration(tsid.viz, 'world/com', x_com.tolist() + [0, 0, 0, 1.])
vizutils.applyViewerConfiguration(tsid.viz, 'world/com_ref', x_com_ref.tolist() + [0, 0, 0, 1.])
vizutils.applyViewerConfiguration(tsid.viz, 'world/rf', pin.SE3ToXYZQUATtuple(H_rf))
vizutils.applyViewerConfiguration(tsid.viz, 'world/lf', pin.SE3ToXYZQUATtuple(H_lf))
vizutils.applyViewerConfiguration(tsid.viz, 'world/rf_ref', x_rf_ref.tolist() + [0, 0, 0, 1.])
vizutils.applyViewerConfiguration(tsid.viz, 'world/lf_ref', x_lf_ref.tolist() + [0, 0, 0, 1.])
if i % 100 == 0 and t>4:
print ('current com', tsid.robot.com(tsid.formulation.data()))
print ('desired com', CurveSet.com_traj[i-time_offset])
if tsid.formulation.checkContact(tsid.contactRF.name, sol):
f = tsid.formulation.getContactForce(tsid.contactRF.name, sol)
print("\tnormal force %s: %.1f"%(tsid.contactRF.name.ljust(20,'.'), tsid.contactRF.getNormalForce(f)))
if tsid.formulation.checkContact(tsid.contactLF.name, sol):
f = tsid.formulation.getContactForce(tsid.contactLF.name, sol)
print("\tnormal force %s: %.1f"%(tsid.contactLF.name.ljust(20,'.'), tsid.contactLF.getNormalForce(f)))
q0, v0 = controller.q0, controller.v0
N = controller.refU.shape[0]
if conf.use_viewer:
robot.initViewer(loadModel=True)
robot.viewer.gui.createSceneWithFloor('world')
robot.viewer.gui.setLightingMode('world/floor', 'OFF')
robot.viewer.gui.setCameraTransform(robot.viz.windowID, conf.CAMERA_TRANSFORM)
robot.display(q0)
for cf in conf.contact_frames:
robot.viewer.gui.addSphere('world/'+cf, conf.SPHERE_RADIUS, conf.SPHERE_COLOR)
H_cf = robot.framePlacement(q0, robot.model.getFrameId(cf))
# print(cf, H_cf.translation)
robot.viewer.gui.applyConfiguration('world/'+cf, pin.SE3ToXYZQUATtuple(H_cf))
robot.viewer.gui.refresh()
data = {}
for simu_params in SIMU_PARAMS:
name = simu_params['name']
print("\nStart simulation", name)
if(simu_params['use_exp_int']):
data[name] = run_simulation(conf, dt, N, robot, controller, q0, v0, simu_params)
else:
data[name] = run_simulation(conf, dt, N, robot, controller, q0, v0, simu_params)
import consim_py.utils.visualize as visualize
from example_robot_data.robots_loader import getModelPath, getVisualPath
def run_simu():
global push_robot_active
i, t = 0, 0.0
q, v = tsid.q, tsid.v
time_avg = 0.0
while True:
time_start = time.time()
tsid.comTask.setReference(tsid.trajCom.computeNext())
tsid.postureTask.setReference(tsid.trajPosture.computeNext())
tsid.rightFootTask.setReference(tsid.trajRF.computeNext())
tsid.leftFootTask.setReference(tsid.trajLF.computeNext())
HQPData = tsid.formulation.computeProblemData(t, q, v)
sol = tsid.solver.solve(HQPData)
if (sol.status != 0):
print("QP problem could not be solved! Error code:", sol.status)
break
# tau = tsid.formulation.getActuatorForces(sol)
dv = tsid.formulation.getAccelerations(sol)
q, v = tsid.integrate_dv(q, v, dv, conf.dt)
i, t = i + 1, t + conf.dt
if (push_robot_active):
push_robot_active = False
data = tsid.formulation.data()
if (tsid.contact_LF_active):
J_LF = tsid.contactLF.computeMotionTask(0.0, q, v, data).matrix
else:
J_LF = np.zeros((0, tsid.model.nv))
if (tsid.contact_RF_active):
J_RF = tsid.contactRF.computeMotionTask(0.0, q, v, data).matrix
else:
J_RF = np.zeros((0, tsid.model.nv))
J = np.vstack((J_LF, J_RF))
J_com = tsid.comTask.compute(t, q, v, data).matrix
A = np.vstack((J_com, J))
b = np.vstack(
(np.array(push_robot_com_vel).T, np.zeros((J.shape[0], 1))))
v = np.linalg.lstsq(A, b, rcond=-1)[0]
if i % conf.DISPLAY_N == 0:
tsid.robot_display.display(q)
x_com = tsid.robot.com(tsid.formulation.data())
x_com_ref = tsid.trajCom.getSample(t).pos()
H_lf = tsid.robot.position(tsid.formulation.data(), tsid.LF)
H_rf = tsid.robot.position(tsid.formulation.data(), tsid.RF)
x_lf_ref = tsid.trajLF.getSample(t).pos()[:3]
x_rf_ref = tsid.trajRF.getSample(t).pos()[:3]
tsid.gui.applyConfiguration('world/com',
x_com.tolist() + [0, 0, 0, 1.])
tsid.gui.applyConfiguration('world/com_ref',
x_com_ref.tolist() + [0, 0, 0, 1.])
tsid.gui.applyConfiguration('world/rf',
pin.SE3ToXYZQUATtuple(H_rf))
tsid.gui.applyConfiguration('world/lf',
pin.SE3ToXYZQUATtuple(H_lf))
tsid.gui.applyConfiguration('world/rf_ref',
x_rf_ref.tolist() + [0, 0, 0, 1.])
tsid.gui.applyConfiguration('world/lf_ref',
x_lf_ref.tolist() + [0, 0, 0, 1.])
if i % 1000 == 0:
print("Average loop time: %.1f (expected is %.1f)" %
(1e3 * time_avg, 1e3 * conf.dt))
time_spent = time.time() - time_start
time_avg = (i * time_avg + time_spent) / (i + 1)
if (time_avg < 0.9 * conf.dt): time.sleep(conf.dt - time_avg)
conf.use_viewer = 1
if conf.use_viewer:
robot.initViewer(loadModel=True)
robot.viewer.gui.createSceneWithFloor('world')
robot.viewer.gui.setLightingMode('world/floor', 'OFF')
robot.viewer.gui.setCameraTransform(robot.viz.windowID,
conf.CAMERA_TRANSFORM)
robot.display(q0)
for cf in conf.contact_frames:
robot.viewer.gui.addSphere('world/' + cf, conf.SPHERE_RADIUS,
conf.SPHERE_COLOR)
H_cf = robot.framePlacement(q0, robot.model.getFrameId(cf))
# print(cf, H_cf.translation)
robot.viewer.gui.applyConfiguration('world/' + cf,
pin.SE3ToXYZQUATtuple(H_cf))
robot.viewer.gui.refresh()
data = {}
for simu_params in SIMU_PARAMS:
name = simu_params['name']
print("\nStart simulation", name)
data[name] = run_simulation(conf, dt, N, robot, controller, q0, v0,
simu_params)
print("max vel %.1f" % np.max(np.abs(data[name].v)))
# COMPUTE INTEGRATION ERRORS:
#res = compute_integration_errors(data, robot, dt)
# PLOT STUFF
line_styles = 100 * ['-o', '--o', '-.o', ':o']
