def callback(q):
q = np.matrix(q).T
gv.applyConfiguration('world/box', pio.se3ToXYZQUATtuple(Mtarget))
gv.applyConfiguration('world/blue',
pio.se3ToXYZQUATtuple(robot.placement(q, 6)))
robot.display(q)
time.sleep(1e-2)
def __init__(self, robot, q0=None, dt=1e-3, ndt=10):
'''
Initialize from a Hrp2Reduced robot model, an initial configuration,
a timestep dt and a number of Euler integration step ndt.
The <simu> method (later defined) processes <ndt> steps, each of them lasting <dt>/<ndt> seconds,
(i.e. total integration time when calling simu is <dt> seconds).
<q0> should be an attribute of robot if it is not given.
'''
self.t = 0.0
self.first_iter = True
self.friction_cones_enabled = False
self.friction_cones_max_violation = 0.0
self.friction_cones_violated = False
self.slippage_allowed = 1
self.mu = 1.0
self.left_foot_contact = True
self.right_foot_contact = True
self.tauc = np.array([0.0, 0.0, 0.0, 0.0]) # coulomb stiction
self.joint_torques_cut_frequency = 30.0
self.tauq = zero(robot.model.nv)
self.frf, self.dfrf = zero(6), zero(6)
self.flf, self.dflf = zero(6), zero(6)
self.dt = dt # Time step
self.ndt = ndt # Discretization (number of calls of step per time step)
self.robot = robot
self.useViewer = robot.useViewer
self.NQ = robot.model.nq
self.NV = robot.model.nv
self.NB = robot.model.nbodies
self.RF = robot.model.getFrameId('rankle')
self.LF = robot.model.getFrameId('lankle')
self.RK = robot.model.frames[self.RF].parent
self.LK = robot.model.frames[self.LF].parent
# NQ,NV,NB,RF,LF,RK,LK = self.NQ,self.NV,self.NB,self.RF,self.LF,self.RK,self.LK
# Tx Ty Tz Rx Ry RZ
#Hrp2 6d stiffness : (4034, 23770, 239018, 707, 502, 936)
# Ty Tz Rx
self.Krf = -np.diagflat([23770, 239018., 0.
]) # Stiffness of the Right spring
self.Klf = -np.diagflat([23770, 239018., 0.
]) # Stiffness of the Left spring
self.Brf = -np.diagflat([50., 500., 0.]) # damping of the Right spring
self.Blf = -np.diagflat([50., 500., 0.]) # damping of the Left spring
if q0 is None: q0 = robot.q0
self.init(q0, reset_contact_positions=True)
if self.useViewer:
robot.viewer.addXYZaxis('world/mrf', [1., .6, .6, 1.], .03, .1)
robot.viewer.addXYZaxis('world/mlf', [.6, .6, 1., 1.], .03, .1)
robot.viewer.applyConfiguration('world/mrf',
se3.se3ToXYZQUATtuple(self.Mrf0))
robot.viewer.applyConfiguration('world/mlf',
se3.se3ToXYZQUATtuple(self.Mlf0))
def callback(q):
q = np.matrix(q).T
gv.applyConfiguration('world/box', pio.se3ToXYZQUATtuple(Mtarget))
for i in range(4):
p_i = robot.framePlacement(q, robot.feetIndexes[i])
gv.applyConfiguration('world/' + colors[i], pio.se3ToXYZQUATtuple(p_i))
gv.applyConfiguration('world/%s_des' % colors[i],
tuple(pdes[i].flat) + (0, 0, 0, 1))
robot.display(q)
time.sleep(1e-1)
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.framesKinematics(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 display(self,q):
pio.forwardKinematics(self.model,self.data,q)
pio.updateGeometryPlacements(self.model,self.data,self.gmodel,self.gdata)
if self.viewer is None: return
for i,g in enumerate(self.gmodel.geometryObjects):
self.viewer.applyConfiguration(g.name, pio.se3ToXYZQUATtuple(self.gdata.oMg[i]))
self.viewer.refresh()
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 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):
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.framesKinematics(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 place(self, objName, M, refresh=True):
'''
This function places (ie changes both translation and rotation) of the object
names "objName" in place given by the SE3 object "M". By default, immediately refresh
the layout. If multiple objects have to be placed at the same time, do the refresh
only at the end of the list.
'''
self.viewer.gui.applyConfiguration(objName, pin.se3ToXYZQUATtuple(M))
if refresh: self.viewer.gui.refresh()
def place(self,objName,M,refresh=True):
'''
This function places (ie changes both translation and rotation) of the object
names "objName" in place given by the SE3 object "M". By default, immediately refresh
the layout. If multiple objects have to be placed at the same time, do the refresh
only at the end of the list.
'''
self.viewer.gui.applyConfiguration(objName,
pin.se3ToXYZQUATtuple(M))
if refresh: self.viewer.gui.refresh()
def refresh(self):
"""
@brief Refresh the configuration of Robot in the viewer.
"""
if Viewer._backend_obj is None or (self.is_backend_parent
and self._backend_proc.poll()
is not None):
raise RuntimeError(
"No backend available. Please start one before calling this method."
)
if not self._lock.acquire(timeout=0.2):
raise RuntimeError("Impossible to acquire backend lock.")
if Viewer.backend == 'gepetto-gui':
if self._rb.displayCollisions:
self._client.applyConfigurations(
[self._getViewerNodeName(collision, pin.GeometryType.COLLISION)
for collision in self._rb.collision_model.geometryObjects],
[pin.se3ToXYZQUATtuple(self._rb.collision_data.oMg[\
self._rb.collision_model.getGeometryId(collision.name)])
for collision in self._rb.collision_model.geometryObjects]
)
if self._rb.displayVisuals:
self._updateGeometryPlacements(visual=True)
self._client.applyConfigurations(
[self._getViewerNodeName(visual, pin.GeometryType.VISUAL)
for visual in self._rb.visual_model.geometryObjects],
[pin.se3ToXYZQUATtuple(self._rb.visual_data.oMg[\
self._rb.visual_model.getGeometryId(visual.name)])
for visual in self._rb.visual_model.geometryObjects]
)
self._client.refresh()
else:
self._updateGeometryPlacements(visual=True)
for visual in self._rb.visual_model.geometryObjects:
T = self._rb.visual_data.oMg[\
self._rb.visual_model.getGeometryId(visual.name)].homogeneous
self._client.viewer[\
self._getViewerNodeName(visual, pin.GeometryType.VISUAL)].set_transform(T)
self._lock.release()
def display(self, xs, fs=[], ps=[], dts=[], factor=1.):
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)
def refresh(self):
"""
@brief Refresh the configuration of Robot in the viewer.
"""
if self.use_theoretical_model:
raise RuntimeError(
"'Refresh' method only available if 'use_theoretical_model'=False."
)
if Viewer.backend == 'gepetto-gui':
with self._lock:
if self._rb.displayCollisions:
self._client.applyConfigurations(
[self._getViewerNodeName(collision, pin.GeometryType.COLLISION)
for collision in self._rb.collision_model.geometryObjects],
[pin.se3ToXYZQUATtuple(self._rb.collision_data.oMg[\
self._rb.collision_model.getGeometryId(collision.name)])
for collision in self._rb.collision_model.geometryObjects]
)
if self._rb.displayVisuals:
self._updateGeometryPlacements(visual=True)
self._client.applyConfigurations(
[self._getViewerNodeName(visual, pin.GeometryType.VISUAL)
for visual in self._rb.visual_model.geometryObjects],
[pin.se3ToXYZQUATtuple(self._rb.visual_data.oMg[\
self._rb.visual_model.getGeometryId(visual.name)])
for visual in self._rb.visual_model.geometryObjects]
)
self._client.refresh()
else:
self._updateGeometryPlacements(visual=True)
for visual in self._rb.visual_model.geometryObjects:
T = self._rb.visual_data.oMg[\
self._rb.visual_model.getGeometryId(visual.name)].homogeneous
self._client.viewer[\
self._getViewerNodeName(visual, pin.GeometryType.VISUAL)].set_transform(T)
def createRandomState(fullBody, limbsInContact):
extraDof = int(fullBody.client.robot.getDimensionExtraConfigSpace())
q0 = fullBody.referenceConfig[::]
if extraDof > 0:
q0 += [0] * extraDof
qr = fullBody.shootRandomConfig()
root = SE3.Identity()
root.translation = np.matrix(qr[0:3]).T
# sample random orientation along z :
root = sampleRotationAlongZ(root)
q0[0:7] = se3ToXYZQUATtuple(root)
# apply random config to legs (FIXME : ID hardcoded for Talos)
q0[7:19] = qr[7:19]
fullBody.setCurrentConfig(q0)
s0 = State(fullBody, q=q0, limbsIncontact=limbsInContact)
return s0
def displayContact(self,contact,contactRef=0,refresh=False):
'''
Display a small red disk at the position of the contact, perpendicular to the
contact normal.
@param contact: the contact object, taken from Pinocchio (HPP-FCL) e.g.
geomModel.collisionResults[0].geContact(0).
@param contactRef: use patch named "world/cparch%d" % contactRef, 0 by default.
@param refresh: option to refresh the viewer before returning, False by default.
'''
name='world/cpatch%d'%contactRef
if self.viewer is None: return
self.viewer.setVisibility(name,'ON')
M = pio.SE3(pio.Quaternion.FromTwoVectors(np.matrix([0,0,1]).T,contact.normal).matrix(),contact.pos)
self.viewer.applyConfiguration(name,pio.se3ToXYZQUATtuple(M))
if refresh: self.viewer.refresh()
def sampleRandomTranstionFromState(fullBody, s0, limbsInContact, movingLimb,
z):
it = 0
success = False
n = [0, 0, 1]
vz = np.matrix(n).T
while not success and it < 10000:
it += 1
# sample a random position for movingLimb and try to project s0 to this position
qr = fullBody.shootRandomConfig()
q1 = s0.q()[::]
q1[limb_ids[movingLimb][0]:limb_ids[movingLimb][1]] = qr[
limb_ids[movingLimb][0]:limb_ids[movingLimb][1]]
s1 = State(fullBody, q=q1, limbsIncontact=limbsInContact)
fullBody.setCurrentConfig(s1.q())
p = fullBody.getJointPosition(
fullBody.dict_limb_joint[movingLimb])[0:3]
p[0] = random.uniform(eff_x_range[0], eff_x_range[1])
p[1] = random.uniform(eff_y_range[0], eff_y_range[1])
p[2] = z
s1, success = StateHelper.addNewContact(s1, movingLimb, p, n)
if success:
"""
# force root orientation : (align current z axis with vertical)
quat_1 = Quaternion(s1.q()[6],s1.q()[3],s1.q()[4],s1.q()[5])
v_1 = quat_1.matrix() * vz
align = Quaternion.FromTwoVectors(v_1,vz)
rot = align * quat_1
q_root = s1.q()[0:7]
q_root[3:7] = rot.coeffs().T.tolist()[0]
"""
root = SE3.Identity()
root.translation = np.matrix(s1.q()[0:3]).T
root = sampleRotationAlongZ(root)
success = s1.projectToRoot(se3ToXYZQUATtuple(root))
# check if new state is in static equilibrium
if success:
# check stability
success = fullBody.isStateBalanced(s1.sId, 3)
if success:
success = projectInKinConstraints(fullBody, s1)
# check if transition is feasible according to CROC
if success:
#success = fullBody.isReachableFromState(s0.sId,s1.sId) or (len(fullBody.isDynamicallyReachableFromState(s0.sId,s1.sId, numPointsPerPhases=0)) > 0)
success = fullBody.isReachableFromState(s0.sId, s1.sId)
return success, s1
def add_contact_surface(self, name, pos, normal, K, B, mu):
''' Add a contact surface (i.e., a wall) located at "pos", with normal
outgoing direction "normal", 3d stiffness K, 3d damping B.
'''
self.contact_surfaces += [ContactSurface(name, pos, normal, K, B, mu)]
# visualize surface in viewer
if(self.gui):
self.gui.addFloor('world/'+name)
self.gui.setLightingMode('world/'+name, 'OFF')
z = np.array([0.,0.,1.])
axis = np.cross(normal, z)
if(norm(axis)>1e-6):
angle = math.atan2(np.linalg.norm(axis), normal.dot(z))
aa = se3.AngleAxis(angle, axis)
H = se3.SE3(aa.matrix(), pos)
self.gui.applyConfiguration('world/'+name, se3.se3ToXYZQUATtuple(H))
else:
self.gui.applyConfiguration('world/'+name, pos.tolist()+[0.,0.,0.,1.])
robot.display(q)
print("The robot is display with end effector on the red box.")
time.sleep(2)
#
# MOVE #############################################################
#
print("Let's start the movement ...")
# Random velocity of the robot driving the movement
vq = np.matrix([ 2.,0,0,4.,0,0]).T
idx = robot.index('wrist_3_joint')
oMeff = robot.placement(q, idx) # Placement of end-eff wrt world at current configuration
oMbox = pio.XYZQUATToSe3(q_box) # Placement of box wrt world
effMbox = oMeff.inverse() * oMbox # Placement of box wrt eff
for i in range(1000):
# Chose new configuration of the robot
q += vq / 40
q[2] = 1.71 + math.sin(i * 0.05) / 2
# Gets the new position of the box
oMbox = robot.placement(q, idx) * effMbox
# Display new configuration for robot and box
gv.applyConfiguration(boxID, pio.se3ToXYZQUATtuple(oMbox))
robot.display(q)
time.sleep(0.1)
]
simu = RobotSimulator(conf, robot)
# visualize surface in the viewer ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ #
if (simu.gui):
simu.gui.addFloor('world/' + name)
simu.gui.setLightingMode('world/' + name, 'OFF')
z = np.array([0., 0., 1.])
axis = np.cross(conf.contact_normal, z)
if (norm(axis) > 1e-6):
angle = math.atan2(np.linalg.norm(axis),
conf.contact_normal.dot(z))
aa = pin.AngleAxis(angle, axis)
H = pin.SE3(aa.matrix(), conf.contact_surface_pos)
simu.gui.applyConfiguration('world/' + name,
pin.se3ToXYZQUATtuple(H))
else:
simu.gui.applyConfiguration(
'world/' + name,
conf.contact_surface_pos.tolist() + [0., 0., 0., 1.])
# ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ #
nq, nv = robot.nq, robot.nv # join position and velocity size
n = nq + nv # state size
m = robot.na # control size
U = np.zeros((N, m)) # initial guess for control inputs
x0 = np.concatenate((conf.q0, np.zeros(robot.nv))) # initial state
# ODE problem definition
ode = ODERobot_wc('ode', robot, contact_points, contact_surfaces)
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 = matlib.zeros((0, tsid.model.nv))
if (tsid.contact_RF_active):
J_RF = tsid.contactRF.computeMotionTask(0.0, q, v, data).matrix
else:
J_RF = matlib.zeros((0, tsid.model.nv))
J = matlib.vstack((J_LF, J_RF))
J_com = tsid.comTask.compute(t, q, v, data).matrix
A = matlib.vstack((J_com, J))
b = matlib.vstack(
(np.matrix(push_robot_com_vel).T, matlib.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()).A1
x_com_ref = tsid.trajCom.getSample(t).pos().A1
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().A1[:3]
x_rf_ref = tsid.trajRF.getSample(t).pos().A1[: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)
This function load a UR5 robot n a new model, move the basis to placement <M0>
and add the corresponding visuals in gepetto viewer with name prefix given by string <name>.
It returns the robot wrapper (model,data).
'''
robot = loadUR()
robot.model.jointPlacements[1] = M0 * robot.model.jointPlacements[1]
robot.visual_model.geometryObjects[0].placement = M0 * robot.visual_model.geometryObjects[0].placement
robot.visual_data.oMg[0] = M0 * robot.visual_data.oMg[0]
robot.initViewer(loadModel=True, sceneName="world/" + name)
return robot
robots = []
# Load 4 Ur5 robots, placed at 0.3m from origin in the 4 directions x,y,-x,-y.
Mt = pio.SE3(eye(3), np.matrix([.3, 0, 0]).T) # First robot is simply translated
for i in range(4):
basePlacement = pio.SE3(rotate('z', np.pi / 2 * i), zero(3)) * Mt
robots.append(loadRobot(basePlacement, "robot%d" % i))
gv = robots[0].viewer.gui # any robots could be use to get gv.
# Set up the robots configuration with end effector pointed upward.
q0 = np.matrix([np.pi / 4, -np.pi / 4, -np.pi / 2, np.pi / 4, np.pi / 2, 0]).T
for i in range(4):
robots[i].display(q0)
# Add a new object featuring the parallel robot tool plate.
[w, h, d] = [0.5, 0.5, 0.005]
color = [red, green, blue, transparency] = [1, 1, 0.78, .3]
gv.addBox('world/robot0/toolplate', w, h, d, color)
Mtool = pio.SE3(rotate('z', 1.268), np.matrix([0, 0, .75]).T)
gv.applyConfiguration('world/robot0/toolplate', pio.se3ToXYZQUATtuple(Mtool))
gv.refresh()
def place(self, obj_name, m):
self.viewer.gui.applyConfiguration(obj_name, pin.se3ToXYZQUATtuple(m))
def place(self, obj_name, m):
self.viewer.gui.applyConfiguration(obj_name,pin.se3ToXYZQUATtuple(m))
