diff --git a/Code/INSTALL b/Code/INSTALL new file mode 100644 index 0000000..2f72b78 --- /dev/null +++ b/Code/INSTALL @@ -0,0 +1,23 @@ +INSTALLATION INSTRUCTIONS + +Robotpkg + +sudo apt-get install cmake cmake-curses-gui cmake-qt-gui curl doxygen g++ ipython libassimp-dev assimp-utils libboost-dev omniidl-python omniorb omniorb-idl omniorb-nameserver python python-matplotlib python-numpy python-scipy python2.7 qgit libbz2-dev zlib1g-dev libssl-dev pax tar libeigen3-dev libtinyxml-dev liburdfdom-dev libboost-all-dev libpcre3-dev libopenscenegraph-dev libqt4-dev python-qt4-dev libomniorb4-dev libqt4-opengl-dev python-omniorb python-scipy + +git clone https://git.openrobots.org/robots/robotpkg.git +cd robotpkg +git clone git://git.openrobots.org/robots/robotpkg/robotpkg-wip wip +export INSTALL_PREFIX=/usr/local/insa/openrobots +./bootstrap/bootstrap --prefix=$INSTALL_PREFIX +cd math/pinocchio; make install +cd graphics/collada-dom; make install +cd wip/osg-dae; make install +cd graphics/gepetto-viewer-corba; make install + +Anaconda + + - dependencies: + - pinocchio, + - gepetto-viewer-corba, + - numpy, scipy, + - matplotlib diff --git a/Code/config.sh b/Code/config.sh new file mode 100644 index 0000000..609e5bf --- /dev/null +++ b/Code/config.sh @@ -0,0 +1,6 @@ +export PYTHONPATH=/usr/local/insa/openrobots/lib/python2.7/site-packages +export LD_LIBRARY_PATH=/usr/local/insa/openrobots/lib +export PATH=/usr/local/insa/openrobots/bin:/usr/local/insa/openrobots/sbin:/bin + +alias python='/usr/bin/python' +alias ipython='/usr/bin/ipython' diff --git a/Code/display.py b/Code/display.py new file mode 100644 index 0000000..021f2af --- /dev/null +++ b/Code/display.py @@ -0,0 +1,61 @@ +# Typical header of a Python script using Pinocchio +from pinocchio.utils import * +from pinocchio.explog import exp,log +from numpy.linalg import pinv,norm +from pinocchio import SE3ToXYZQUATtuple +import gepetto.corbaserver + +# Example of a class Display that connect to Gepetto-viewer and implement a +# 'place' method to set the position/rotation of a 3D visual object in a scene. +class Display(): + ''' + Class Display: Example of a class implementing a client for the Gepetto-viewer server. The main + method of the class is 'place', that sets the position/rotation of a 3D visual object in a scene. + ''' + def __init__(self,windowName = "pinocchio" ): + ''' + This function connect with the Gepetto-viewer server and open a window with the given name. + If the window already exists, it is kept in the current state. Otherwise, the newly-created + window is set up with a scene named 'world'. + ''' + + # Create the client and connect it with the display server. + try: + self.viewer=gepetto.corbaserver.Client() + except: + print("Error while starting the viewer client. ") + print("Check whether Gepetto-viewer is properly started") + + # Open a window for displaying your model. + try: + # If the window already exists, do not do anything. + windowID = self.viewer.gui.getWindowID (windowName) + print("Warning: window '"+windowName+"' already created.") + print("The previously created objects will not be destroyed and do not have to be created again.") + except: + # Otherwise, create the empty window. + windowID = self.viewer.gui.createWindow (windowName) + # Start a new "scene" in this window, named "world", with just a floor. + self.viewer.gui.createSceneWithFloor("world") + self.viewer.gui.addSceneToWindow("world",windowID) + + # Finally, refresh the layout to obtain your first rendering. + self.viewer.gui.refresh() + + def nofloor(self): + ''' + This function will hide the floor. + ''' + self.viewer.gui.setVisibility('world/floor',"OFF") + 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, SE3ToXYZQUATtuple(M)) + if refresh: self.viewer.gui.refresh() + diff --git a/Code/example_display.py b/Code/example_display.py new file mode 100644 index 0000000..fa95c1c --- /dev/null +++ b/Code/example_display.py @@ -0,0 +1,30 @@ +# Example of creation of an object of class Display (implemented in the previous example, inside +# a file 'display.py'). Do not forget to start Gepetto-viewer server in another terminal before +# creating the client. + +import pinocchio as se3 +from pinocchio.utils import * + +from display import Display +display = Display() + + +# Example of use of the class Display to create a box visual object. +boxid = 147 +name = 'box' + str(boxid) +[w,h,d] = [1.0,1.0,1.0] +color = [red,green,blue,transparency] = [1,1,0.78,1.0] +display.viewer.gui.addBox('world/'+name, w,h,d,color) + +# Example of use of the class Display to create a sphere visual object. +display.viewer.gui.addSphere('world/sphere', 1.0,color) + +# Example of use of the class Display to create a cylinder visual object. +radius = 1.0 +height = 1.0 +display.viewer.gui.addCylinder('world/cylinder', radius,height,color) + +# Example of use of the class display to place the previously-create object at random SE3 placements. +display.place("world/box147",se3.SE3.Random(),False) +display.place("world/sphere",se3.SE3.Random(),False) +display.place("world/cylinder",se3.SE3.Random()) diff --git a/Code/example_robot.py b/Code/example_robot.py new file mode 100644 index 0000000..508e6b3 --- /dev/null +++ b/Code/example_robot.py @@ -0,0 +1,27 @@ +# Create a 7DOF manipulator robot and moves it along a constant (random) velocity during 10secs. +import numpy as np +from numpy.linalg import pinv,norm +from pinocchio import neutral +from robot_arm import Robot +import time + +# Create a 7DOF robot. +robot = Robot() + +# Hide the floor. +robot.viewer.viewer.gui.setVisibility('world/floor','OFF') + +# Move the robot during 10secs at velocity v. +dt = 1e-3 +for j in range (robot.model.nv): + v = np.array (robot.model.nv * [0]) + v [j] = 1 + q = neutral (robot.model) + for i in range(1000): + q += v*dt + robot.display(q) + time.sleep(dt) + for i in range(1000): + q -= v*dt + robot.display(q) + time.sleep(dt) diff --git a/Code/example_scipy.py b/Code/example_scipy.py new file mode 100644 index 0000000..4986eec --- /dev/null +++ b/Code/example_scipy.py @@ -0,0 +1,39 @@ +import numpy as np +from scipy.optimize import fmin_bfgs, fmin_slsqp + +def cost(x): + '''Cost f(x,y) = x^2 + 2y^2 - 2xy - 2x ''' + x0 = x[0] + x1 = x[1] + return -1*(2*x0*x1 + 2*x0 - x0**2 - 2*x1**2) + +def constraint_eq(x): + ''' Constraint x^3 = y ''' + return np.array([ x[0]**3-x[1] ]) + +def constraint_ineq(x): + '''Constraint x>=2, y>=2''' + return np.array([ x[0]-2,x[1]-2 ]) + +class CallbackLogger: + def __init__(self): + self.nfeval = 1 + def __call__(self,x): + print('===CBK=== {0:4d} {1: 3.6f} {2: 3.6f} {3: 3.6f}'.format(self.nfeval, x[0], x[1], cost(x))) + self.nfeval += 1 + +x0 = np.array([0.0,0.0]) +# Optimize cost without any constraints in BFGS, with traces. +xopt_bfgs = fmin_bfgs(cost, x0, callback=CallbackLogger()) +print('\n *** Xopt in BFGS = ',xopt_bfgs,'\n\n\n\n') + +# Optimize cost without any constraints in CLSQ +xopt_lsq = fmin_slsqp(cost,[-1.0,1.0], iprint=2, full_output=1) +print('\n *** Xopt in LSQ = ',xopt_lsq,'\n\n\n\n') + +# Optimize cost with equality and inequality constraints in CLSQ +xopt_clsq = fmin_slsqp(cost,[-1.0,1.0], + f_eqcons=constraint_eq, f_ieqcons=constraint_ineq, + iprint=2, full_output=1) +print('\n *** Xopt in c-lsq = ',xopt_clsq,'\n\n\n\n') + diff --git a/Code/inverse_kinematics.py b/Code/inverse_kinematics.py new file mode 100644 index 0000000..4dc457e --- /dev/null +++ b/Code/inverse_kinematics.py @@ -0,0 +1,37 @@ +import numpy as np +import numpy.linalg +from scipy.optimize import fmin_bfgs +from pinocchio import forwardKinematics, log, neutral +import eigenpy +eigenpy.switchToNumpyMatrix() + +class CallbackLogger: + def __init__(self, ik): + self.nfeval = 1 + self.ik = ik + def __call__(self,x): + print '===CBK=== {0:4d} {1}'.format(self.nfeval, + self.ik.latestCost) + self.nfeval += 1 + +class InverseKinematics (object): + leftFootJoint = 'left_leg_6_joint' + rightFootJoint = 'right_leg_6_joint' + waistJoint = 'waist_joint' + + def __init__ (self, robot): + self.q = neutral (robot.model) + forwardKinematics (robot.model, robot.data, self.q) + self.robot = robot + # Initialize references of feet and center of mass with initial values + self.leftFootRefPose = robot.data.oMi [robot.leftFootJointId].copy () + self.rightFootRefPose = robot.data.oMi [robot.rightFootJointId].copy () + self.waistRefPose = robot.data.oMi [robot.waistJointId].copy () + + def cost (self, q): + # Write your code here + pass + + def solve (self, q): + # Write your code here + pass diff --git a/Code/legged_robot.py b/Code/legged_robot.py new file mode 100644 index 0000000..ec9235f --- /dev/null +++ b/Code/legged_robot.py @@ -0,0 +1,53 @@ +from pinocchio.utils import * +from pinocchio.explog import exp,log +from numpy.linalg import pinv,norm +from pinocchio import SE3, Model, Inertia, JointModelFreeFlyer, JointModelRX, \ + JointModelRY, JointModelRZ, forwardKinematics, neutral +import gepetto.corbaserver +from display import Display + +class Visual: + ''' + Class representing one 3D mesh of the robot, to be attached to a joint. The class contains: + * the name of the 3D objects inside Gepetto viewer. + * the ID of the joint in the kinematic tree to which the body is attached. + * the placement of the body with respect to the joint frame. + This class is only used in the list Robot.visuals (see below). + ''' + def __init__(self,name,jointParent,placement): + self.name = name # Name in gepetto viewer + self.jointParent = jointParent # ID (int) of the joint + self.placement = placement # placement of the body wrt joint, i.e. bodyMjoint + def place(self,display,oMjoint): + oMbody = oMjoint*self.placement + display.place(self.name,oMbody,False) + +class Robot: + ''' + Define a class Robot representing a biped robot + The members of the class are: + * viewer: a display encapsulating a gepetto viewer client to create 3D + objects and place them. + * model: the kinematic tree of the robot. + * data: the temporary variables to be used by the kinematic algorithms. + * visuals: the list of all the 'visual' 3D objects to render the robot, + each element of the list being an object Visual (see above). + ''' + + def __init__(self): + self.viewer = Display() + self.visuals = [] + self.model = Model () + self.createLeggedRobot () + self.data = self.model.createData() + self.q0 = neutral (self.model) + + def createLeggedRobot (self,rootId=0, prefix=''): + # Write your code here + pass + + def display(self,q): + forwardKinematics(self.model,self.data,q) + for visual in self.visuals: + visual.place( self.viewer,self.data.oMi[visual.jointParent] ) + self.viewer.viewer.gui.refresh() diff --git a/Code/robot_arm.py b/Code/robot_arm.py new file mode 100644 index 0000000..e45245b --- /dev/null +++ b/Code/robot_arm.py @@ -0,0 +1,90 @@ +from pinocchio.utils import * +from pinocchio.explog import exp,log +from numpy.linalg import pinv,norm +from pinocchio import forwardKinematics, Inertia, JointModelRX, Model, SE3 +import gepetto.corbaserver +from display import Display +import eigenpy +eigenpy.switchToNumpyArray() + +class Visual: + ''' + Class representing one 3D mesh of the robot, to be attached to a joint. The class contains: + * the name of the 3D objects inside Gepetto viewer. + * the ID of the joint in the kinematic tree to which the body is attached. + * the placement of the body with respect to the joint frame. + This class is only used in the list Robot.visuals (see below). + ''' + def __init__(self,name,jointParent,placement): + self.name = name # Name in gepetto viewer + self.jointParent = jointParent # ID (int) of the joint + self.placement = placement # placement of the body wrt joint, i.e. bodyMjoint + def place(self,display,oMjoint): + oMbody = oMjoint*self.placement + display.place(self.name,oMbody,False) + +class Robot: + ''' + Define a class Robot with 7DOF (shoulder=3 + elbow=1 + wrist=3). + The configuration is nq=7. The velocity is the same. + The members of the class are: + * viewer: a display encapsulating a gepetto viewer client to create 3D objects and place them. + * model: the kinematic tree of the robot. + * data: the temporary variables to be used by the kinematic algorithms. + * visuals: the list of all the 'visual' 3D objects to render the robot, each element of the list being + an object Visual (see above). + + See tp1.py for an example of use. + ''' + + def __init__(self): + self.viewer = Display() + self.visuals = [] + self.model = Model () + self.createArm3DOF() + self.data = self.model.createData() + self.q0 = zero(self.model.nq) + + def createArm3DOF(self,rootId=0, prefix='', jointPlacement=SE3.Identity()): + color = [red,green,blue,transparency] = [1,1,0.78,1.0] + colorred = [1.0,0.0,0.0,1.0] + + jointId = rootId + + jointName = prefix + "shoulder_joint" + joint = JointModelRX() + jointId = self.model.addJoint(jointId,joint,jointPlacement,jointName) + self.model.appendBodyToJoint(jointId,Inertia.Random(),SE3.Identity()) + self.viewer.viewer.gui.addSphere('world/' + prefix + 'shoulder', 0.3,colorred) + self.visuals.append( Visual('world/' + prefix + 'shoulder',jointId,SE3.Identity()) ) + self.viewer.viewer.gui.addBox('world/' + prefix + 'upperarm', .1,.1,.5,color) + self.visuals.append( Visual('world/' + prefix + 'upperarm',jointId,SE3(eye(3),np.array([0., 0., .5])))) + + jointName = prefix + "elbow_joint" + jointPlacement = SE3(eye(3),np.array( [0, 0, 1.0] )) + joint = JointModelRX() + jointId = self.model.addJoint(jointId,joint,jointPlacement,jointName) + self.model.appendBodyToJoint(jointId,Inertia.Random(),SE3.Identity()) + self.viewer.viewer.gui.addSphere('world/' + prefix + 'elbow', 0.3,colorred) + self.visuals.append( Visual('world/' + prefix + 'elbow',jointId,SE3.Identity()) ) + self.viewer.viewer.gui.addBox('world/' + prefix + 'lowerarm', .1,.1,.5,color) + self.visuals.append( Visual('world/' + prefix + 'lowerarm',jointId,SE3(eye(3),np.array([0., 0., .5])))) + + jointName = prefix + "wrist_joint" + jointPlacement = SE3(eye(3),np.array( [0, 0, 1.0] )) + joint = JointModelRX() + jointId = self.model.addJoint(jointId,joint,jointPlacement,jointName) + self.model.appendBodyToJoint(jointId,Inertia.Random(),SE3.Identity()) + self.viewer.viewer.gui.addSphere('world/' + prefix + 'wrist', 0.3,colorred) + self.visuals.append( Visual('world/' + prefix + 'wrist',jointId,SE3.Identity()) ) + self.viewer.viewer.gui.addBox('world/' + prefix + 'hand', .1,.1,.25,color) + self.visuals.append( Visual('world/' + prefix + 'hand',jointId,SE3(eye(3),np.array([0., 0., .25])))) + + + def display(self,q): + forwardKinematics(self.model,self.data,q) + for visual in self.visuals: + visual.place( self.viewer,self.data.oMi[visual.jointParent] ) + self.viewer.viewer.gui.refresh() + +