Commit d0c89989 authored by Florent Lamiraux's avatar Florent Lamiraux Committed by Florent Lamiraux
Browse files

Add incomplete files and

parent 68d4dbda
import numpy as np
import numpy.linalg
from scipy.optimize import fmin_bfgs
from pinocchio import forwardKinematics, log, neutral
import eigenpy
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.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,, self.q)
self.robot = robot
# Initialize references of feet and center of mass with initial values
self.leftFootRefPose = [robot.leftFootJointId].copy ()
self.rightFootRefPose = [robot.rightFootJointId].copy ()
self.waistRefPose = [robot.waistJointId].copy ()
def cost (self, q):
# Write your code here
def solve (self, q):
# Write your code here
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): = 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,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.model.createData()
self.q0 = neutral (self.model)
def createLeggedRobot (self,rootId=0, prefix=''):
# Write your code here
def display(self,q):
for visual in self.visuals: self.viewer,[visual.jointParent] )
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment