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']