Commit e7345401 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

Merge tag 'v1.2.0'

Release of version 1.2.0.
parents 7036c61c 08e31310
......@@ -70,10 +70,16 @@ IF(EIGEN_NO_AUTOMATIC_RESIZING)
ENDIF(EIGEN_NO_AUTOMATIC_RESIZING)
# ----------------------------------------------------
# --- DEPENDANCIES -----------------------------------
# --- DEPENDENCIES -----------------------------------
# ----------------------------------------------------
ADD_REQUIRED_DEPENDENCY("eigen3 >= 3.2.0") # Eigen::Ref appeared from 3.2.0
ADD_REQUIRED_DEPENDENCY("pinocchio >= 1.3.0")
# Fail-safe support for catkin-ized pinocchio:
# - If catkin-based pinocchio is installed it runs the CFG_EXTRAS to set up the Pinocchio preprocessor directives
# - If it isn't, nothing happens and the subsequent pkg-config check takes care of everything.
#find_package(catkin QUIET COMPONENTS pinocchio)
ADD_REQUIRED_DEPENDENCY("pinocchio >= 2.0.0")
SET(BOOST_REQUIERED_COMPONENTS filesystem system)
SET(BOOST_BUILD_COMPONENTS unit_test_framework)
......@@ -82,7 +88,8 @@ SET(BOOST_OPTIONAL_COMPONENTS "")
IF(BUILD_PYTHON_INTERFACE)
SET(BOOST_OPTIONAL_COMPONENTS ${BOOST_OPTIONAL_COMPONENTS} python)
FINDPYTHON()
INCLUDE_DIRECTORIES(SYSTEM ${PYTHON_INCLUDE_DIRS})
INCLUDE_DIRECTORIES(SYSTEM ${PYTHON_INCLUDE_DIRS})
ADD_REQUIRED_DEPENDENCY("eigenpy >= 1.4.0")
ENDIF(BUILD_PYTHON_INTERFACE)
SET(BOOST_COMPONENTS ${BOOST_REQUIERED_COMPONENTS} ${BOOST_OPTIONAL_COMPONENTS} ${BOOST_BUILD_COMPONENTS})
......
......@@ -48,6 +48,7 @@ SET(PYWRAP ${PYWRAP} PARENT_SCOPE)
# --- DEPENDENCIES --- #
SET(PKG_CONFIG_PYWRAP_REQUIRES "eigenpy >= 1.4.0")
SET(PKG_CONFIG_PYWRAP_REQUIRES "pinocchio >= 2.0.0")
FOREACH(dep ${PKG_CONFIG_PYWRAP_REQUIRES})
ADD_COMPILE_DEPENDENCY(${dep})
ENDFOREACH(dep ${PKG_CONFIG_PYWRAP_REQUIRES})
......@@ -110,6 +111,7 @@ ENDIF(BUILD_WITH_COMMIT_VERSION)
ADD_HEADER_GROUP(${PYWRAP}_HEADERS)
ADD_SOURCE_GROUP(${PYWRAP}_SOURCES)
PKG_CONFIG_USE_DEPENDENCY(${PYWRAP} eigenpy)
PKG_CONFIG_USE_DEPENDENCY(${PYWRAP} pinocchio)
TARGET_LINK_LIBRARIES(${PYWRAP} ${PROJECT_NAME})
TARGET_LINK_BOOST_PYTHON(${PYWRAP})
......
......@@ -81,17 +81,17 @@ namespace tsid
return name;
}
static math::ConstraintEquality computeMotionTask(Contact6d & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const se3::Data & data){
static math::ConstraintEquality computeMotionTask(Contact6d & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const pinocchio::Data & data){
self.computeMotionTask(t, q, v, data);
math::ConstraintEquality cons(self.getMotionConstraint().name(), self.getMotionConstraint().matrix(), self.getMotionConstraint().vector());
return cons;
}
static math::ConstraintInequality computeForceTask(Contact6d & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const se3::Data & data){
static math::ConstraintInequality computeForceTask(Contact6d & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const pinocchio::Data & data){
self.computeForceTask(t, q, v, data);
math::ConstraintInequality cons(self.getForceConstraint().name(), self.getForceConstraint().matrix(), self.getForceConstraint().lowerBound(), self.getForceConstraint().upperBound());
return cons;
}
static math::ConstraintEquality computeForceRegularizationTask(Contact6d & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const se3::Data & data){
static math::ConstraintEquality computeForceRegularizationTask(Contact6d & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const pinocchio::Data & data){
self.computeForceRegularizationTask(t, q, v, data);
math::ConstraintEquality cons(self.getForceRegularizationTask().name(), self.getForceRegularizationTask().matrix(), self.getForceRegularizationTask().vector());
return cons;
......@@ -127,7 +127,7 @@ namespace tsid
static bool setMaxNormalForce (Contact6d & self, const double maxNormalForce){
return self.setMaxNormalForce(maxNormalForce);
}
static void setReference(Contact6d & self, const se3::SE3 & ref){
static void setReference(Contact6d & self, const pinocchio::SE3 & ref){
self.setReference(ref);
}
static void setForceReference(Contact6d & self, const::Eigen::VectorXd f_ref){
......
......@@ -80,17 +80,17 @@ namespace tsid
return name;
}
static math::ConstraintEquality computeMotionTask(ContactPoint & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const se3::Data & data){
static math::ConstraintEquality computeMotionTask(ContactPoint & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const pinocchio::Data & data){
self.computeMotionTask(t, q, v, data);
math::ConstraintEquality cons(self.getMotionConstraint().name(), self.getMotionConstraint().matrix(), self.getMotionConstraint().vector());
return cons;
}
static math::ConstraintInequality computeForceTask(ContactPoint & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const se3::Data & data){
static math::ConstraintInequality computeForceTask(ContactPoint & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const pinocchio::Data & data){
self.computeForceTask(t, q, v, data);
math::ConstraintInequality cons(self.getForceConstraint().name(), self.getForceConstraint().matrix(), self.getForceConstraint().lowerBound(), self.getForceConstraint().upperBound());
return cons;
}
static math::ConstraintEquality computeForceRegularizationTask(ContactPoint & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const se3::Data & data){
static math::ConstraintEquality computeForceRegularizationTask(ContactPoint & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const pinocchio::Data & data){
self.computeForceRegularizationTask(t, q, v, data);
math::ConstraintEquality cons(self.getForceRegularizationTask().name(), self.getForceRegularizationTask().matrix(), self.getForceRegularizationTask().vector());
return cons;
......@@ -129,7 +129,7 @@ namespace tsid
static bool setMaxNormalForce (ContactPoint & self, const double maxNormalForce){
return self.setMaxNormalForce(maxNormalForce);
}
static void setReference(ContactPoint & self, const se3::SE3 & ref){
static void setReference(ContactPoint & self, const pinocchio::SE3 & ref){
self.setReference(ref);
}
static void setForceReference(ContactPoint & self, const::Eigen::VectorXd f_ref){
......
......@@ -72,8 +72,8 @@ namespace tsid
.def("getContactForce", &InvDynPythonVisitor::getContactForce, bp::args("name", "HQPOutput"))
;
}
static se3::Data data(const T & self){
se3::Data data = self.data();
static pinocchio::Data data(const T & self){
pinocchio::Data data = self.data();
return data;
}
static bool addMotionTask_SE3(T & self, tasks::TaskSE3Equality & task, double weight, unsigned int priorityLevel, double transition_duration){
......
......@@ -44,7 +44,7 @@ namespace tsid
{
cl
.def(bp::init<std::string, std_vec, bool>((bp::arg("filename"), bp::arg("package_dir"), bp::arg("verbose")), "Default constructor without RootJoint."))
.def(bp::init<std::string, std_vec, se3::JointModelVariant, bool>((bp::arg("filename"), bp::arg("package_dir"), bp::arg("roottype"), bp::arg("verbose")), "Default constructor without RootJoint."))
.def(bp::init<std::string, std_vec, pinocchio::JointModelVariant, bool>((bp::arg("filename"), bp::arg("package_dir"), bp::arg("roottype"), bp::arg("verbose")), "Default constructor without RootJoint."))
.add_property("nq", &Robot::nq)
.add_property("nv", &Robot::nv)
......@@ -74,11 +74,11 @@ namespace tsid
;
}
static se3::Model model (const Robot & self){
static pinocchio::Model model (const Robot & self){
return self.model();
}
static se3::Data data(const Robot & self){
se3::Data data(self.model());
static pinocchio::Data data(const Robot & self){
pinocchio::Data data(self.model());
return data;
}
static Eigen::VectorXd rotor_inertias(const Robot & self){
......@@ -94,46 +94,46 @@ namespace tsid
return self.gear_ratios(gear_ratios);
}
static Eigen::Vector3d com (const Robot & self, const se3::Data & data){
static Eigen::Vector3d com (const Robot & self, const pinocchio::Data & data){
return self.com(data);
}
static Eigen::Vector3d com_vel (const Robot & self, const se3::Data & data){
static Eigen::Vector3d com_vel (const Robot & self, const pinocchio::Data & data){
return self.com_vel(data);
}
static Eigen::Vector3d com_acc (const Robot & self, const se3::Data & data){
static Eigen::Vector3d com_acc (const Robot & self, const pinocchio::Data & data){
return self.com_acc(data);
}
static Matrix3x Jcom (const Robot & self, const se3::Data & data){
static Matrix3x Jcom (const Robot & self, const pinocchio::Data & data){
return self.Jcom(data);
}
static void computeAllTerms (const Robot & self, se3::Data & data, const Eigen::VectorXd & q, const Eigen::VectorXd & v){
static void computeAllTerms (const Robot & self, pinocchio::Data & data, const Eigen::VectorXd & q, const Eigen::VectorXd & v){
self.computeAllTerms(data, q, v);
}
static Eigen::MatrixXd mass (Robot & self, se3::Data & data){
static Eigen::MatrixXd mass (Robot & self, pinocchio::Data & data){
return self.mass(data);
}
static Eigen::VectorXd nonLinearEffects(const Robot & self, const se3::Data & data){
static Eigen::VectorXd nonLinearEffects(const Robot & self, const pinocchio::Data & data){
return self.nonLinearEffects(data);
}
static se3::SE3 position(const Robot & self, const se3::Data & data, const se3::Model::JointIndex & index){
static pinocchio::SE3 position(const Robot & self, const pinocchio::Data & data, const pinocchio::Model::JointIndex & index){
return self.position(data, index);
}
static se3::Motion velocity(const Robot & self, const se3::Data & data, const se3::Model::JointIndex & index){
static pinocchio::Motion velocity(const Robot & self, const pinocchio::Data & data, const pinocchio::Model::JointIndex & index){
return self.velocity(data, index);
}
static se3::Motion acceleration(const Robot & self, const se3::Data & data, const se3::Model::JointIndex & index){
static pinocchio::Motion acceleration(const Robot & self, const pinocchio::Data & data, const pinocchio::Model::JointIndex & index){
return self.acceleration(data, index);
}
static se3::SE3 framePosition(const Robot & self, const se3::Data & data, const se3::Model::FrameIndex & index){
static pinocchio::SE3 framePosition(const Robot & self, const pinocchio::Data & data, const pinocchio::Model::FrameIndex & index){
return self.framePosition(data, index);
}
static se3::Motion frameVelocity(const Robot & self, const se3::Data & data, const se3::Model::FrameIndex & index){
static pinocchio::Motion frameVelocity(const Robot & self, const pinocchio::Data & data, const pinocchio::Model::FrameIndex & index){
return self.frameVelocity(data, index);
}
static se3::Motion frameAcceleration(const Robot & self, const se3::Data & data, const se3::Model::FrameIndex & index){
static pinocchio::Motion frameAcceleration(const Robot & self, const pinocchio::Data & data, const pinocchio::Model::FrameIndex & index){
return self.frameAcceleration(data, index);
}
static se3::Motion frameClassicAcceleration(const Robot & self, const se3::Data & data, const se3::Model::FrameIndex & index){
static pinocchio::Motion frameClassicAcceleration(const Robot & self, const pinocchio::Data & data, const pinocchio::Model::FrameIndex & index){
return self.frameClassicAcceleration(data, index);
}
......
......@@ -69,7 +69,7 @@ namespace tsid
std::string name = self.name();
return name;
}
static math::ConstraintEquality compute(TaskCOM & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const se3::Data & data){
static math::ConstraintEquality compute(TaskCOM & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const pinocchio::Data & data){
self.compute(t, q, v, data);
math::ConstraintEquality cons(self.getConstraint().name(), self.getConstraint().matrix(), self.getConstraint().vector());
return cons;
......@@ -132,4 +132,4 @@ namespace tsid
}
#endif // ifndef __tsid_python_task_com_hpp__
\ No newline at end of file
#endif // ifndef __tsid_python_task_com_hpp__
......@@ -71,7 +71,7 @@ namespace tsid
std::string name = self.name();
return name;
}
static math::ConstraintEquality compute(TaskJoint & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const se3::Data & data){
static math::ConstraintEquality compute(TaskJoint & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const pinocchio::Data & data){
self.compute(t, q, v, data);
math::ConstraintEquality cons(self.getConstraint().name(), self.getConstraint().matrix(), self.getConstraint().vector());
return cons;
......@@ -140,4 +140,4 @@ namespace tsid
}
#endif // ifndef __tsid_python_task_joint_hpp__
\ No newline at end of file
#endif // ifndef __tsid_python_task_joint_hpp__
......@@ -70,7 +70,7 @@ namespace tsid
std::string name = self.name();
return name;
}
static math::ConstraintEquality compute(TaskSE3 & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const se3::Data & data){
static math::ConstraintEquality compute(TaskSE3 & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const pinocchio::Data & data){
self.compute(t, q, v, data);
math::ConstraintEquality cons(self.getConstraint().name(), self.getConstraint().matrix(), self.getConstraint().vector());
return cons;
......@@ -136,4 +136,4 @@ namespace tsid
}
#endif // ifndef __tsid_python_task_se3_hpp__
\ No newline at end of file
#endif // ifndef __tsid_python_task_se3_hpp__
......@@ -41,7 +41,7 @@ namespace tsid
{
cl
.def(bp::init<std::string>((bp::arg("name")), "Default Constructor with name"))
.def(bp::init<std::string, se3::SE3>((bp::arg("name"), bp::arg("reference")), "Default Constructor with name and ref_vec"))
.def(bp::init<std::string, pinocchio::SE3>((bp::arg("name"), bp::arg("reference")), "Default Constructor with name and ref_vec"))
.add_property("size", &TrajSE3::size)
.def("setReference", &TrajectorySE3ConstantPythonVisitor::setReference, bp::arg("M_ref"))
......@@ -51,7 +51,7 @@ namespace tsid
.def("getSample", &TrajectorySE3ConstantPythonVisitor::getSample, bp::arg("time"))
;
}
static void setReference(TrajSE3 & self, const se3::SE3 & ref){
static void setReference(TrajSE3 & self, const pinocchio::SE3 & ref){
self.setReference(ref);
}
static trajectories::TrajectorySample computeNext(TrajSE3 & self){
......@@ -81,4 +81,4 @@ namespace tsid
}
#endif // ifndef __tsid_python_traj_se3_hpp__
\ No newline at end of file
#endif // ifndef __tsid_python_traj_se3_hpp__
Subproject commit 8e7bedfcbd8524c0401a58fd74edc07c3d4308d0
Subproject commit 1d9aeca25e970d2d967fd5be0fb93fe961db121b
......@@ -47,7 +47,7 @@ vector.extend(item for item in path)
robot = tsid.RobotWrapper(urdf, vector, se3.JointModelFreeFlyer(), False)
# for gepetto viewer
robot_display = se3.RobotWrapper(urdf, [path, ], se3.JointModelFreeFlyer())
robot_display = se3.RobotWrapper.BuildFromURDF(urdf, [path, ], se3.JointModelFreeFlyer())
l = commands.getstatusoutput("ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l")
if int(l[1]) == 0:
os.system('gepetto-gui &')
......
......@@ -27,7 +27,7 @@ lf_frame_name = "LAnkleRoll" # left foot frame name
contactNormal = np.matrix([0., 0., 1.]).T # direction of the normal to the contact surface
w_com = 1.0 # weight of center of mass task
w_posture = 1e-3 # weight of joint posture task
w_forceRef = 1e-5 # weight of force regularization task
w_forceReg = 1e-5 # weight of force regularization task
w_RF = 1.0 # weight of right foot motion task
kp_contact = 30.0 # proportional gain of contact constraint
kp_com = 30.0 # proportional gain of center of mass task
......@@ -51,7 +51,7 @@ robot = tsid.RobotWrapper(urdf, vector, se3.JointModelFreeFlyer(), False)
srdf = path + '/srdf/romeo_collision.srdf'
# for gepetto viewer .. but Fix me!!
robot_display = se3.RobotWrapper(urdf, [path, ], se3.JointModelFreeFlyer())
robot_display = se3.RobotWrapper.BuildFromURDF(urdf, [path, ], se3.JointModelFreeFlyer())
l = commands.getstatusoutput("ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l")
if int(l[1]) == 0:
......@@ -61,7 +61,7 @@ cl = gepetto.corbaserver.Client()
gui = cl.gui
robot_display.initDisplay(loadModel=True)
q = se3.getNeutralConfigurationFromSrdf(robot.model(), srdf, False)
q = se3.getNeutralConfiguration(robot.model(), srdf, False)
q[2] += 0.84
v = np.matrix(np.zeros(robot.nv)).transpose()
......@@ -80,19 +80,19 @@ contact_Point = np.matrix(np.ones((3,4)) * lz)
contact_Point[0, :] = [-lxn, -lxn, lxp, lxp]
contact_Point[1, :] = [-lyn, lyp, -lyn, lyp]
contactRF =tsid.Contact6d("contact_rfoot", robot, rf_frame_name, contact_Point, contactNormal, mu, fMin, fMax, w_forceRef)
contactRF =tsid.Contact6d("contact_rfoot", robot, rf_frame_name, contact_Point, contactNormal, mu, fMin, fMax)
contactRF.setKp(kp_contact * np.matrix(np.ones(6)).transpose())
contactRF.setKd(2.0 * np.sqrt(kp_contact) * np.matrix(np.ones(6)).transpose())
H_rf_ref = robot.position(data, robot.model().getJointId(rf_frame_name))
contactRF.setReference(H_rf_ref)
invdyn.addRigidContact(contactRF)
invdyn.addRigidContact(contactRF, w_forceReg)
contactLF =tsid.Contact6d("contact_lfoot", robot, lf_frame_name, contact_Point, contactNormal, mu, fMin, fMax, w_forceRef)
contactLF =tsid.Contact6d("contact_lfoot", robot, lf_frame_name, contact_Point, contactNormal, mu, fMin, fMax)
contactLF.setKp(kp_contact * np.matrix(np.ones(6)).transpose())
contactLF.setKd(2.0 * np.sqrt(kp_contact) * np.matrix(np.ones(6)).transpose())
H_lf_ref = robot.position(data, robot.model().getJointId(lf_frame_name))
contactLF.setReference(H_lf_ref)
invdyn.addRigidContact(contactLF)
invdyn.addRigidContact(contactLF, w_forceReg)
comTask = tsid.TaskComEquality("task-com", robot)
comTask.setKp(kp_com * np.matrix(np.ones(3)).transpose())
......
......@@ -52,7 +52,7 @@ robot = tsid.RobotWrapper(urdf, vector, se3.JointModelFreeFlyer(), False)
srdf = path + '/srdf/romeo_collision.srdf'
# for gepetto viewer
robot_display = se3.RobotWrapper(urdf, [path, ], se3.JointModelFreeFlyer())
robot_display = se3.RobotWrapper.BuildFromURDF(urdf, [path, ], se3.JointModelFreeFlyer())
l = commands.getstatusoutput("ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l")
if int(l[1]) == 0:
os.system('gepetto-gui &')
......@@ -61,7 +61,7 @@ cl = gepetto.corbaserver.Client()
gui = cl.gui
robot_display.initDisplay(loadModel=True)
q = se3.getNeutralConfigurationFromSrdf(robot.model(), srdf, False)
q = se3.getNeutralConfiguration(robot.model(), srdf, False)
q[2] += 0.84
v = np.matrix(np.zeros(robot.nv)).T
......@@ -80,19 +80,19 @@ contact_Point = np.matrix(np.ones((3,4)) * lz)
contact_Point[0, :] = [-lxn, -lxn, lxp, lxp]
contact_Point[1, :] = [-lyn, lyp, -lyn, lyp]
contactRF =tsid.Contact6d("contact_rfoot", robot, rf_frame_name, contact_Point, contactNormal, mu, fMin, fMax, w_forceRef)
contactRF =tsid.Contact6d("contact_rfoot", robot, rf_frame_name, contact_Point, contactNormal, mu, fMin, fMax)
contactRF.setKp(kp_contact * matlib.ones(6).T)
contactRF.setKd(2.0 * np.sqrt(kp_contact) * matlib.ones(6).T)
H_rf_ref = robot.position(data, robot.model().getJointId(rf_frame_name))
contactRF.setReference(H_rf_ref)
invdyn.addRigidContact(contactRF)
invdyn.addRigidContact(contactRF, w_forceRef)
contactLF =tsid.Contact6d("contact_lfoot", robot, lf_frame_name, contact_Point, contactNormal, mu, fMin, fMax, w_forceRef)
contactLF =tsid.Contact6d("contact_lfoot", robot, lf_frame_name, contact_Point, contactNormal, mu, fMin, fMax)
contactLF.setKp(kp_contact * matlib.ones(6).T)
contactLF.setKd(2.0 * np.sqrt(kp_contact) * matlib.ones(6).T)
H_lf_ref = robot.position(data, robot.model().getJointId(lf_frame_name))
contactLF.setReference(H_lf_ref)
invdyn.addRigidContact(contactLF)
invdyn.addRigidContact(contactLF, w_forceRef)
comTask = tsid.TaskComEquality("task-com", robot)
comTask.setKp(kp_com * matlib.ones(3).T)
......
......@@ -41,7 +41,7 @@ kp_posture = 10.0 # proportional gain of joint posture task
dt = 0.001 # controller time step
PRINT_N = 500 # print every PRINT_N time steps
DISPLAY_N = 25 # update robot configuration in viwewer every DISPLAY_N time steps
N_SIMULATION = 40000 # number of time steps simulated
N_SIMULATION = 4000 # number of time steps simulated
filename = str(os.path.dirname(os.path.abspath(__file__)))
path = filename + '/../models/romeo'
......@@ -52,7 +52,7 @@ robot = tsid.RobotWrapper(urdf, vector, se3.JointModelFreeFlyer(), False)
srdf = path + '/srdf/romeo_collision.srdf'
# for gepetto viewer
robot_display = se3.RobotWrapper(urdf, [path, ], se3.JointModelFreeFlyer())
robot_display = se3.RobotWrapper.BuildFromURDF(urdf, [path, ], se3.JointModelFreeFlyer())
l = commands.getstatusoutput("ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l")
if int(l[1]) == 0:
os.system('gepetto-gui &')
......@@ -61,7 +61,7 @@ cl = gepetto.corbaserver.Client()
gui = cl.gui
robot_display.initDisplay(loadModel=True)
q = se3.getNeutralConfigurationFromSrdf(robot.model(), srdf, False)
q = se3.getNeutralConfiguration(robot.model(), srdf, False)
q[2] += 0.84
v = np.matrix(np.zeros(robot.nv)).T
......@@ -80,27 +80,19 @@ contact_Point = np.matrix(np.ones((3,4)) * lz)
contact_Point[0, :] = [-lxn, -lxn, lxp, lxp]
contact_Point[1, :] = [-lyn, lyp, -lyn, lyp]
contactRF =tsid.Contact6d("contact_rfoot", robot, rf_frame_name, contact_Point, contactNormal, mu, fMin, fMax, w_forceRef)
contactRF =tsid.Contact6d("contact_rfoot", robot, rf_frame_name, contact_Point, contactNormal, mu, fMin, fMax)
contactRF.setKp(kp_contact * matlib.ones(6).T)
contactRF.setKd(2.0 * np.sqrt(kp_contact) * matlib.ones(6).T)
H_rf_ref = robot.position(data, robot.model().getJointId(rf_frame_name))
contactRF.setReference(H_rf_ref)
invdyn.addRigidContact(contactRF)
invdyn.addRigidContact(contactRF, w_forceRef)
contactLF =tsid.Contact6d("contact_lfoot", robot, lf_frame_name, contact_Point, contactNormal, mu, fMin, fMax, w_forceRef)
contactLF =tsid.Contact6d("contact_lfoot", robot, lf_frame_name, contact_Point, contactNormal, mu, fMin, fMax)
contactLF.setKp(kp_contact * matlib.ones(6).T)
contactLF.setKd(2.0 * np.sqrt(kp_contact) * matlib.ones(6).T)
H_lf_ref = robot.position(data, robot.model().getJointId(lf_frame_name))
contactLF.setReference(H_lf_ref)
invdyn.addRigidContact(contactLF)
lh_frame_name = 'LWristPitch'
contactLH =tsid.Contact6d("contact_lhand", robot, lh_frame_name, contact_Point, contactNormal, mu, fMin, fMax, w_forceRef)
contactLH.setKp(kp_contact * matlib.ones(6).T)
contactLH.setKd(2.0 * np.sqrt(kp_contact) * matlib.ones(6).T)
H_lh_ref = robot.position(data, robot.model().getJointId(lh_frame_name))
contactLH.setReference(H_lh_ref)
invdyn.addRigidContact(contactLH)
invdyn.addRigidContact(contactLF, w_forceRef)
comTask = tsid.TaskComEquality("task-com", robot)
comTask.setKp(kp_com * matlib.ones(3).T)
......@@ -133,7 +125,7 @@ com_acc_des = matlib.empty((3, N_SIMULATION))*nan # acc_des = acc_ref - Kp*pos_e
offset = robot.com(data)
amp = np.matrix([0.0, 0.05, 0.0]).T
two_pi_f = 2*np.pi*np.matrix([0.0, 1.1, 0.0]).T
two_pi_f = 2*np.pi*np.matrix([0.0, 0.5, 0.0]).T
two_pi_f_amp = np.multiply(two_pi_f,amp)
two_pi_f_squared_amp = np.multiply(two_pi_f, two_pi_f_amp)
......
# -*- coding: utf-8 -*-
"""
Created on Fri Jan 16 09:16:56 2015
@author: adelpret
"""
import matplotlib as mpl
import matplotlib.pyplot as plt
import matplotlib.ticker as ticker
import numpy as np
DEFAULT_FONT_SIZE = 20;
DEFAULT_AXIS_FONT_SIZE = DEFAULT_FONT_SIZE;
DEFAULT_LINE_WIDTH = 4; #13;
DEFAULT_MARKER_SIZE = 6;
DEFAULT_FONT_FAMILY = 'sans-serif'
DEFAULT_FONT_SIZE = DEFAULT_FONT_SIZE;
DEFAULT_FONT_SERIF = ['Times New Roman', 'Times','Bitstream Vera Serif', 'DejaVu Serif', 'New Century Schoolbook', 'Century Schoolbook L', 'Utopia', 'ITC Bookman', 'Bookman', 'Nimbus Roman No9 L', 'Palatino', 'Charter', 'serif'];
DEFAULT_FIGURE_FACE_COLOR = 'white' # figure facecolor; 0.75 is scalar gray
DEFAULT_LEGEND_FONT_SIZE = DEFAULT_FONT_SIZE;
DEFAULT_AXES_LABEL_SIZE = DEFAULT_FONT_SIZE; # fontsize of the x any y labels
DEFAULT_TEXT_USE_TEX = False;
LINE_ALPHA = 0.9;
SAVE_FIGURES = False;
FILE_EXTENSIONS = ['png']; #'pdf']; #['png']; #,'eps'];
FIGURES_DPI = 150;
SHOW_LEGENDS = False;
LEGEND_ALPHA = 0.5;
SHOW_FIGURES = False;
FIGURE_PATH = './';
LINE_WIDTH_RED = 0; # reduction of line width when plotting multiple lines on same plot
LINE_WIDTH_MIN = 1;
BOUNDS_COLOR = 'silver';
#legend.framealpha : 1.0 # opacity of of legend frame
#axes.hold : True # whether to clear the axes by default on
#axes.linewidth : 1.0 # edge linewidth
#axes.titlesize : large # fontsize of the axes title
#axes.color_cycle : b, g, r, c, m, y, k # color cycle for plot lines
#xtick.labelsize : medium # fontsize of the tick labels
#figure.dpi : 80 # figure dots per inch
#image.cmap : jet # gray | jet etc...
#savefig.dpi : 100 # figure dots per inch
#savefig.facecolor : white # figure facecolor when saving
#savefig.edgecolor : white # figure edgecolor when saving
#savefig.format : png # png, ps, pdf, svg
#savefig.jpeg_quality: 95 # when a jpeg is saved, the default quality parameter.
#savefig.directory : ~ # default directory in savefig dialog box,
# leave empty to always use current working directory
def create_empty_figure(nRows=1, nCols=1, spinesPos=None,sharex=True):
f, ax = plt.subplots(nRows,nCols,sharex=sharex);
mngr = plt.get_current_fig_manager()
# mngr.window.setGeometry(50,50,1080,720);
if(spinesPos!=None):
if(nRows*nCols>1):
for axis in ax.reshape(nRows*nCols):
movePlotSpines(axis, spinesPos);
else:
movePlotSpines(ax, spinesPos);
return (f, ax);
def movePlotSpines(ax, spinesPos):
ax.spines['right'].set_color('none')
ax.spines['top'].set_color('none')
ax.xaxis.set_ticks_position('bottom')
ax.spines['bottom'].set_position(('data',spinesPos[0]))
ax.yaxis.set_ticks_position('left')
ax.spines['left'].set_position(('data',spinesPos[1]))
def setAxisFontSize(ax, size):
for label in ax.get_xticklabels() + ax.get_yticklabels():
label.set_fontsize(size)
label.set_bbox(dict(facecolor='white', edgecolor='None', alpha=0.65))
mpl.rcdefaults()
mpl.rcParams['lines.linewidth'] = DEFAULT_LINE_WIDTH;
mpl.rcParams['lines.markersize'] = DEFAULT_MARKER_SIZE;
mpl.rcParams['patch.linewidth'] = 1;
mpl.rcParams['font.family'] = DEFAULT_FONT_FAMILY;
mpl.rcParams['font.size'] = DEFAULT_FONT_SIZE;
mpl.rcParams['font.serif'] = DEFAULT_FONT_SERIF;
mpl.rcParams['text.usetex'] = DEFAULT_TEXT_USE_TEX;
mpl.rcParams['axes.labelsize'] = DEFAULT_AXES_LABEL_SIZE;
mpl.rcParams['legend.fontsize'] = DEFAULT_LEGEND_FONT_SIZE;
mpl.rcParams['figure.facecolor'] = DEFAULT_FIGURE_FACE_COLOR;
mpl.rcParams['figure.figsize'] = 23, 12 #12, 9 #
def plot3dQuantity(quantity, title, ax=None, boundUp=None, boundLow=None, yscale='linear', linestyle='k'):
return plotNdQuantity(3, 1, quantity, title, ax, boundUp, boundLow, yscale, linestyle);
def plotNdQuantity(nRows, nCols, quantity, title="", ax=None, boundUp=None, boundLow=None, yscale='linear',
linestyle='k--', sharey=False, margins=None):
t = quantity.shape[0];
n = quantity.shape[1];
if(margins!=None):
if(type(margins) is list):
margins = [margins[0].reshape(t,1,n), margins[1].reshape(t,1,n)];
else:
margins = margins.reshape(t,1,n);
return plotNdQuantityPerSolver(nRows, nCols, quantity.reshape(t,1,n), title, None, [linestyle], ax,
boundUp, boundLow, yscale, None, None, sharey, margins);
def plotNdQuantityPerSolver(nRows, nCols, quantity, title, solver_names, line_styles, ax=None, boundUp=None, boundLow=None,
yscale='linear', subplot_titles=None, ylabels=None, sharey=False, margins=None, x=None):
if(ax==None):
f, ax = plt.subplots(nRows, nCols, sharex=True, sharey=sharey);
ax = ax.reshape(nRows, nCols);
k = 0;
if(x==None):
x = range(quantity.shape[0]);
for j in range(nCols):
for i in range(nRows):
if(k<quantity.shape[2]):
if(subplot_titles!=None):
ax[i,j].set_title(subplot_titles[k]);
elif(i==0):
ax[i,j].set_title(str(k)); # set titles on first row only