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 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 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 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)