def t_poseDisplay(self):
q = zero(self.model.nq)
q[6] = 1
v = zero(self.model.nv)
# Right Arm
# This is used to obtain the index of the joint that will be rotated
idx = self.model.getJointId('shoulder_r')
idx = self.model.joints[idx].idx_q
# The shoulder is a spherical joint expressed as quaterion to avoid the singularities of Euler angles
# We first rotate the DoF in se3
M = se3.SE3.Identity()
M.rotation = rotate('y', -np.pi / 2)
# Now we convert it in a quaternion
Mquat = se3ToXYZQUAT(M)
q[idx] = Mquat[3]
q[idx + 1] = Mquat[4]
q[idx + 2] = Mquat[5]
q[idx + 3] = Mquat[6]
# Rotate left arm
idx = self.model.getJointId('shoulder_l')
idx = self.model.joints[idx].idx_q
M = se3.SE3.Identity()
M.rotation = rotate('y', np.pi / 2)
Mquat = se3ToXYZQUAT(M)
q[idx] = Mquat[3]
q[idx + 1] = Mquat[4]
q[idx + 2] = Mquat[5]
q[idx + 3] = Mquat[6]
# Now the forward dynamics is computed to obtain the T pose
self.display(q, v)
self.q = q
def PlaceForceArrow(self,
name,
oMc,
cPhic,
color_linear_force=None,
refreshGui=False):
name_linForce = "force_" + name
name_torque = "torque_" + name
linForce = cPhic[:3, 0]
torque = cPhic[3:, 0]
val_linForce = max(LA.norm(linForce), 1e-4)
val_torque = max(LA.norm(torque), 1e-4)
dir_linForce = linForce / val_linForce
dir_torque = torque / val_torque
dir0 = np.matrix([1., 0., 0.]).T
oMlinForce = oMc * se3.SE3(rotation_matrix(dir0, dir_linForce),
zero(3))
oMtorque = oMc * se3.SE3(rotation_matrix(dir0, dir_torque), zero(3))
# divid by gravity to normalize the forces
self.viewer.gui.resizeArrow("world/" + name_linForce,
self.force_arrow_radius,
val_linForce / 728.22)
self.viewer.gui.resizeArrow("world/" + name_torque,
self.force_arrow_radius,
val_torque / 728.22)
# set color for linear force
if color_linear_force is not None:
self.viewer.gui.setColor("world/" + name_linForce,
color_linear_force)
self.viewer.gui.applyConfiguration("world/" + name_linForce,
se3ToXYZQUAT(oMlinForce))
self.viewer.gui.applyConfiguration("world/" + name_torque,
se3ToXYZQUAT(oMtorque))
if refreshGui:
self.viewer.gui.refresh()
def display(self, q):
RobotWrapper.display(self, q)
M1 = self.data.oMi[1]
self.viewer.gui.applyConfiguration('world/mobilebasis', se3ToXYZQUAT(M1))
self.viewer.gui.applyConfiguration('world/mobilewheel1', se3ToXYZQUAT(M1))
self.viewer.gui.applyConfiguration('world/mobilewheel2', se3ToXYZQUAT(M1))
self.viewer.gui.refresh()
se3.framesKinematics(self.model, self.data)
self.viewer.gui.applyConfiguration('world/framebasis', se3ToXYZQUAT(self.data.oMf[-2]))
self.viewer.gui.applyConfiguration('world/frametool', se3ToXYZQUAT(self.data.oMf[-1]))
self.viewer.gui.refresh()
def display_prm(robot, graph):
'''Take a graph object containing a list of configurations q and
a dictionnary of graph relations edge. Display the configurations by the correspond
placement of the robot end effector. Display the graph relation by vertices connecting
the robot end effector positions.
'''
gui = robot.viewer.gui
try:
gui.deleteNode('world/prm', True)
except:
pass
gui.createRoadmap('world/prm', [1., .2, .2, .8], 1e-2, 1e-2,
[1., .2, .2, .8])
for q in graph.q:
gui.addNodeToRoadmap('world/prm', se3ToXYZQUAT(robot.position(q, 6)))
for parent, children in graph.children.items():
for child in children:
if child > parent:
q1, q2 = graph.q[parent], graph.q[child]
p1 = robot.position(q1, 6).translation.ravel().tolist()[0]
p2 = robot.position(q2, 6).translation.ravel().tolist()[0]
gui.addEdgeToRoadmap('world/prm', p1, p2)
gui.refresh()
def display_prm(robot, graph):
'''Take a graph object containing a list of configurations q and
a dictionnary of graph relations edge. Display the configurations by the correspond
placement of the robot end effector. Display the graph relation by vertices connecting
the robot end effector positions.
'''
gui = robot.viewer.gui
try:
gui.deleteNode('world/prm', True)
except:
pass
gui.createRoadmap('world/prm', [1., .2, .2, .8], 1e-2, 1e-2, [1., .2, .2, .8])
for q in graph.q:
gui.addNodeToRoadmap('world/prm', se3ToXYZQUAT(robot.position(q, 6)))
for parent, children in graph.children.items():
for child in children:
if child > parent:
q1, q2 = graph.q[parent], graph.q[child]
p1 = robot.position(q1, 6).translation.ravel().tolist()[0]
p2 = robot.position(q2, 6).translation.ravel().tolist()[0]
gui.addEdgeToRoadmap('world/prm', p1, p2)
gui.refresh()
def on_packet(packet):
""" Callback function that is called everytime a data packet arrives from QTM """
position = packet.get_6d()[1][body_id][0]
orientation = packet.get_6d()[1][body_id][1]
timestamp = packet.timestamp * 1e-6
# Get the last position and Rotation matrix from the shared memory.
last_position = np.array(
[shared_bodyPosition[0], shared_bodyPosition[1], shared_bodyPosition[2]])
last_rotation = np.array([[shared_bodyOrientationMat9[0], shared_bodyOrientationMat9[1], shared_bodyOrientationMat9[2]],
[shared_bodyOrientationMat9[3], shared_bodyOrientationMat9[4], shared_bodyOrientationMat9[5]],
[shared_bodyOrientationMat9[6], shared_bodyOrientationMat9[7], shared_bodyOrientationMat9[8]]])
last_se3 = pinocchio.SE3(last_rotation, last_position)
# Get the new position and Rotation matrix from the motion capture.
position = np.array([position.x, position.y, position.z]) * 1e-3
rotation = np.array(orientation.matrix).reshape(3, 3).transpose()
se3 = pinocchio.SE3(rotation, position)
xyzquat = se3ToXYZQUAT(se3)
# Get the position, Rotation matrix and Quaternion
shared_bodyPosition[0] = xyzquat[0]
shared_bodyPosition[1] = xyzquat[1]
shared_bodyPosition[2] = xyzquat[2]
shared_bodyOrientationQuat[0] = xyzquat[3]
shared_bodyOrientationQuat[1] = xyzquat[4]
shared_bodyOrientationQuat[2] = xyzquat[5]
shared_bodyOrientationQuat[3] = xyzquat[6]
shared_bodyOrientationMat9[0] = orientation.matrix[0]
shared_bodyOrientationMat9[1] = orientation.matrix[3]
shared_bodyOrientationMat9[2] = orientation.matrix[6]
shared_bodyOrientationMat9[3] = orientation.matrix[1]
shared_bodyOrientationMat9[4] = orientation.matrix[4]
shared_bodyOrientationMat9[5] = orientation.matrix[7]
shared_bodyOrientationMat9[6] = orientation.matrix[2]
shared_bodyOrientationMat9[7] = orientation.matrix[5]
shared_bodyOrientationMat9[8] = orientation.matrix[8]
# Compute world velocity.
if (shared_timestamp.value == -1):
shared_bodyVelocity[0] = 0
shared_bodyVelocity[1] = 0
shared_bodyVelocity[2] = 0
shared_bodyAngularVelocity[0] = 0.0
shared_bodyAngularVelocity[1] = 0.0
shared_bodyAngularVelocity[2] = 0.0
else:
dt = timestamp - shared_timestamp.value
shared_bodyVelocity[0] = (position[0] - last_position[0])/dt
shared_bodyVelocity[1] = (position[1] - last_position[1])/dt
shared_bodyVelocity[2] = (position[2] - last_position[2])/dt
bodyAngularVelocity = log(last_se3.rotation.T.dot(se3.rotation))/dt
shared_bodyAngularVelocity[0] = bodyAngularVelocity[0]
shared_bodyAngularVelocity[1] = bodyAngularVelocity[1]
shared_bodyAngularVelocity[2] = bodyAngularVelocity[2]
shared_timestamp.value = timestamp
def rotateSPHJ(self, q, axis, angle, idx):
v = zero(self.model.nv)
M = se3.SE3.Identity()
#M.rotation = self.data.oMi[idx].rotation * rotate(axis, angle)
M.rotation = rotate(axis, angle)
Mquat = se3ToXYZQUAT(M)
for dof in range(idx, idx + 4):
q[dof] = Mquat[3 + dof - idx]
self.play(q, v)
def display(self, q):
RobotWrapper.display(self, q)
M1 = self.data.oMi[1]
self.viewer.gui.applyConfiguration('world/mobilebasis',
se3ToXYZQUAT(M1))
self.viewer.gui.applyConfiguration('world/mobilewheel1',
se3ToXYZQUAT(M1))
self.viewer.gui.applyConfiguration('world/mobilewheel2',
se3ToXYZQUAT(M1))
self.viewer.gui.refresh()
se3.framesKinematics(self.model, self.data)
self.viewer.gui.applyConfiguration('world/framebasis',
se3ToXYZQUAT(self.data.oMf[-2]))
self.viewer.gui.applyConfiguration('world/frametool',
se3ToXYZQUAT(self.data.oMf[-1]))
self.viewer.gui.refresh()
def test_se3ToXYZQUAT_XYZQUATToSe3(self):
m = pin.SE3.Identity()
m.translation = np.matrix('1. 2. 3.').T
m.rotation = np.matrix(
'1. 0. 0.;0. 0. -1.;0. 1. 0.') # rotate('x', pi / 2)
self.assertApprox(
se3ToXYZQUAT(m),
[1., 2., 3., sqrt(2) / 2, 0, 0,
sqrt(2) / 2])
self.assertApprox(
XYZQUATToSe3([1., 2., 3.,
sqrt(2) / 2, 0, 0,
sqrt(2) / 2]), m)
def M2gv(M):
return se3ToXYZQUAT(M)
def test_se3ToXYZQUAT_XYZQUATToSe3(self):
m = pin.SE3.Identity()
m.translation = np.matrix('1. 2. 3.').T
m.rotation = np.matrix('1. 0. 0.;0. 0. -1.;0. 1. 0.') # rotate('x', pi / 2)
self.assertApprox(se3ToXYZQUAT(m).T, [1., 2., 3., sqrt(2) / 2, 0, 0, sqrt(2) / 2])
self.assertApprox(XYZQUATToSe3([1., 2., 3., sqrt(2) / 2, 0, 0, sqrt(2) / 2]), m)
def half_sitting(self):
q = self.q0
q[2] = 0.92 #0.81
v = self.v0
idx = self.model.getJointId('hip_r')
idx = self.model.joints[idx].idx_q
M = se3.SE3.Identity()
M.rotation = rotate('x', 0.2) * rotate('y', -0.05)
Mquat = se3ToXYZQUAT(M)
q[idx] = Mquat[3]
q[idx + 1] = Mquat[4]
q[idx + 2] = Mquat[5]
q[idx + 3] = Mquat[6]
idx = self.model.getJointId('hip_l')
idx = self.model.joints[idx].idx_q
M = se3.SE3.Identity()
M.rotation = rotate('x', 0.2) * rotate('y', 0.05)
Mquat = se3ToXYZQUAT(M)
q[idx] = Mquat[3]
q[idx + 1] = Mquat[4]
q[idx + 2] = Mquat[5]
q[idx + 3] = Mquat[6]
idx = self.model.getJointId('knee_r')
idx = self.model.joints[idx].idx_q
q[idx] = -0.4 #1.22
idx = self.model.getJointId('knee_l')
idx = self.model.joints[idx].idx_q
q[idx] = -0.4
idx = self.model.getJointId('ankle_r')
idx = self.model.joints[idx].idx_q
q[idx] = 0.25 #0.61
idx = self.model.getJointId('ankle_l')
idx = self.model.joints[idx].idx_q
q[idx] = 0.25
idx = self.model.getJointId('mtp_r')
idx = self.model.joints[idx].idx_q
q[idx] = -0.05
idx = self.model.getJointId('mtp_l')
idx = self.model.joints[idx].idx_q
q[idx] = -0.05
# Torso and head
idx = self.model.getJointId('back')
idx = self.model.joints[idx].idx_q
M = se3.SE3.Identity()
M.rotation = rotate('x', -0.2)
Mquat = se3ToXYZQUAT(M)
q[idx] = Mquat[3]
q[idx + 1] = Mquat[4]
q[idx + 2] = Mquat[5]
q[idx + 3] = Mquat[6]
idx = self.model.getJointId('neck')
idx = self.model.joints[idx].idx_q
M = se3.SE3.Identity()
M.rotation = rotate('x', 0.2)
Mquat = se3ToXYZQUAT(M)
q[idx] = Mquat[3]
q[idx + 1] = Mquat[4]
q[idx + 2] = Mquat[5]
q[idx + 3] = Mquat[6]
# upper body
idx = self.model.getJointId('acromial_r')
idx = self.model.joints[idx].idx_q
M = se3.SE3.Identity()
M.rotation = rotate('x', -0.2) * rotate('y', -0.18)
Mquat = se3ToXYZQUAT(M)
q[idx] = Mquat[3]
q[idx + 1] = Mquat[4]
q[idx + 2] = Mquat[5]
q[idx + 3] = Mquat[6]
idx = self.model.getJointId('acromial_l')
idx = self.model.joints[idx].idx_q
M = se3.SE3.Identity()
M.rotation = rotate('x', -0.2) * rotate('y', 0.18)
Mquat = se3ToXYZQUAT(M)
q[idx] = Mquat[3]
q[idx + 1] = Mquat[4]
q[idx + 2] = Mquat[5]
q[idx + 3] = Mquat[6]
idx = self.model.getJointId('elbow_r')
idx = self.model.joints[idx].idx_q
q[idx] = 0.7
idx = self.model.getJointId('elbow_l')
idx = self.model.joints[idx].idx_q
q[idx] = 0.7
idx = self.model.getJointId('lunate_hand_r')
idx = self.model.joints[idx].idx_q
q[idx] = 0.15
idx = self.model.getJointId('lunate_hand_l')
idx = self.model.joints[idx].idx_q
q[idx] = 0.15
idx = self.model.getJointId('radioulnar_r')
idx = self.model.joints[idx].idx_q
q[idx] = 1.
idx = self.model.getJointId('radioulnar_l')
idx = self.model.joints[idx].idx_q
q[idx] = 1. #0.22
idx = self.model.getJointId('radius_lunate_r')
idx = self.model.joints[idx].idx_q
q[idx] = -0.02
idx = self.model.getJointId('radius_lunate_l')
idx = self.model.joints[idx].idx_q
q[idx] = 0.02
self.q = q
return q
def place(self, obj_name, m):
self.viewer.gui.applyConfiguration(obj_name, XYZQUATToViewerConfiguration(se3ToXYZQUAT(m)))
def place(self, obj_name, m):
self.viewer.gui.applyConfiguration(obj_name, XYZQUATToViewerConfiguration(se3ToXYZQUAT(m)))
def fromSE3(M):
return se3ToXYZQUAT(M)
and add the corresponding visuals in gepetto viewer with name prefix given by string <name>.
It returns the robot wrapper (model,data).
'''
robot = RobotWrapper(urdf, [PKG])
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.initDisplay(loadModel=True, viewerRootNodeName="world/" + name)
return robot
robots = []
# Load 4 Ur5 robots, placed at 0.3m from origin in the 4 directions x,y,-x,-y.
Mt = SE3(eye(3), np.matrix([.3, 0, 0]).T) # First robot is simply translated
for i in range(4):
robots.append(loadRobot(SE3(rotate('z', np.pi / 2 * i), zero(3)) * Mt, "robot%d" % i))
# 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.
gepettoViewer = robots[0].viewer.gui
w, h, d = 0.25, 0.25, 0.005
color = [red, green, blue, transparency] = [1, 1, 0.78, 1.0]
gepettoViewer.addBox('world/toolplate', w, h, d, color)
Mtool = SE3(rotate('z', 1.268), np.matrix([0, 0, .77]).T)
gepettoViewer.applyConfiguration('world/toolplate', se3ToXYZQUAT(Mtool))
gepettoViewer.refresh()
import time
import numpy as np
import pinocchio as pin
from pinocchio.utils import eye, se3ToXYZQUAT
import gviewserver
gv = gviewserver.GepettoViewerServer()
gv.addSphere('world/ball', .1, [1, 0, 0, 1]) # radius, color=[r,g,b,1]
gv.addCapsule('world/capsule', .05, .75,
[1, 1, 1, 1]) # radius, length, color = [r,g,b,1]
gv.addBox('world/world/box', .2, .05, .5,
[.5, .5, 1, 1]) # depth(x),length(y),height(z), color
gv.applyConfiguration('world/box',
[.1, .1, .1, 1, 0, 0, 0]) # x,y,z, quaternion
gv.refresh()
time.sleep(1)
M = pin.SE3(eye(3), np.matrix([.2, .2, .2]).T)
gv.applyConfiguration('world/box', se3ToXYZQUAT(M))
gv.refresh()
# gv.deleteNode('world', True)
def callback(q):
q = np.matrix(q).T
robot.display(q) # noqa
gv.applyConfiguration('world/blue', se3ToXYZQUAT(robot.placement(q, 6))) # noqa
time.sleep(.1)
