Classes | Functions
grl::vrep Namespace Reference

Classes

class  InverseKinematicsVrepPlugin
 
class  KukaVrepPlugin
 
class  UniversalRobotsVrepPlugin
 
class  VrepRobotArmDriver
 C++ interface for any open chain V-REP robot arm. More...
 

Functions

int getHandle (const std::string &param)
 
template<std::size_t I, typename Param >
int getHandleFromParam (const Param &params_)
 
template<std::size_t I, typename Params , typename OutputIterator >
OutputIterator getHandleFromParam (const Params params_, OutputIterator out)
 
template<typename SinglePassRange , typename OutputIterator >
OutputIterator getHandles (const SinglePassRange inputRange, OutputIterator out)
 
Eigen::MatrixXf getJacobian (vrep::VrepRobotArmDriver &driver, bool jacobianOnly=false)
 get the Jacobian as calculated by vrep in an Eigen::MatrixXf in column major format More...
 
std::vector< std::string > jointToLink (const std::vector< std::string > &jointNames)
 
std::vector< std::string > jointToLinkRespondable (std::vector< std::string > jointNames)
 
void SetRBDynArmFromVrep (const std::vector< std::string > &vrepJointNames, const std::vector< int > &vrepJointHandles, const rbd::MultiBody &simArmMultiBody, rbd::MultiBodyConfig &simArmConfig, std::string debug="")
 
void SetVRepArmFromRBDyn (const std::vector< std::string > &vrepJointNames, const std::vector< int > &vrepJointHandles, const rbd::MultiBody &simArmMultiBody, const rbd::MultiBodyConfig &simArmConfig, std::string debug="")
 

Function Documentation

§ getHandle()

int grl::vrep::getHandle ( const std::string &  param)

Definition at line 10 of file Vrep.hpp.

§ getHandleFromParam() [1/2]

template<std::size_t I, typename Param >
int grl::vrep::getHandleFromParam ( const Param &  params_)

Definition at line 33 of file Vrep.hpp.

§ getHandleFromParam() [2/2]

template<std::size_t I, typename Params , typename OutputIterator >
OutputIterator grl::vrep::getHandleFromParam ( const Params  params_,
OutputIterator  out 
)
Parameters
params_Params range of handles such as std::vector<std::String>

Definition at line 40 of file Vrep.hpp.

§ getHandles()

template<typename SinglePassRange , typename OutputIterator >
OutputIterator grl::vrep::getHandles ( const SinglePassRange  inputRange,
OutputIterator  out 
)

Definition at line 20 of file Vrep.hpp.

§ getJacobian()

Eigen::MatrixXf grl::vrep::getJacobian ( vrep::VrepRobotArmDriver driver,
bool  jacobianOnly = false 
)

get the Jacobian as calculated by vrep in an Eigen::MatrixXf in column major format

Parameters
driverthe vrep arm driver object that provides access to vrep simulation state for a specific arm
jacobianOnlytrue calls simComputeJacobian, false uses simCheckIKGroup which incidentally calculates the Jacobian

The jacobianOnly option is provided because some irregularities were seen with simComputeJacobian (jacobianOnly == true). Thus (jacobianOnly == false) falls back on using the full vrep provided inverse kinematics calculation which is slower but didn't have the previously seen irregularities. The jacobianOnly option should be eliminated in favor of the simComputeJacobian (jacobianOnly == true) case once all issues are fully resolved.

Returns
jacobian in ColumnMajor format where each row is a joint from base to tip, first 3 columns are translation component, last 3 columns are rotation component
Todo:
get target position, probably relative to base

Run inverse kinematics, but all we really want is the jacobian

See also
http://www.coppeliarobotics.com/helpFiles/en/apiFunctions.htm#simCheckIkGroup
Todo:
do we need to use these options?
See also
http://www.coppeliarobotics.com/helpFiles/en/apiConstants.htm#ikCalculationResults
Todo:
FIX HACK jacobianSize include orientation component, should be 7x6 instead of 7x3

Definition at line 35 of file VrepRobotArmJacobian.hpp.

§ jointToLink()

std::vector<std::string> grl::vrep::jointToLink ( const std::vector< std::string > &  jointNames)
Todo:
TODO(ahundt) joint to link and joint to respondable only works for v-rep provided scene heirarchy names, modify full class so these workarounds aren't needed in other words this assumes everything is named like the following: joint1: LBR_iiwa_14_R820_joint1 link1: LBR_iiwa_14_R820_link1 link1_respondable: LBR_iiwa_14_R820_link1_resp

That assumption is definitely not true and names can be arbitrary so they must be provided through the v-rep lua API.

Todo:
TODO(ahundt) FIX HACK! Manually adding last link

Definition at line 30 of file VrepRobotArmDriver.hpp.

§ jointToLinkRespondable()

std::vector<std::string> grl::vrep::jointToLinkRespondable ( std::vector< std::string >  jointNames)
Todo:
TODO(ahundt) HACK joint to link and joint to respondable only works for v-rep provided scene heirarchy names, modify full class so these workarounds aren't needed.
See also
jointToLink
Todo:
TODO(ahundt) link 1 isn't respondable because it is anchored to the ground, but should there be an empty string so the indexes are consistent or skip it entirely? (currently skipping)

Definition at line 45 of file VrepRobotArmDriver.hpp.

§ SetRBDynArmFromVrep()

void grl::vrep::SetRBDynArmFromVrep ( const std::vector< std::string > &  vrepJointNames,
const std::vector< int > &  vrepJointHandles,
const rbd::MultiBody &  simArmMultiBody,
rbd::MultiBodyConfig &  simArmConfig,
std::string  debug = "" 
)

Sets the VREP simulation joint positions from the RBDyn configuration

Parameters
vrepJointNamesthe name of each joint, order must match vrepJointHandles
vrepJointHandlesthe V-REP simulation object handle for each joint
rbdJointNamesthe names of all the simArmMultiBody joints, which typically has more joints than the VREP list.
simArmMultiBodydefines the structure of the robot arm for the RBDyn library, see RBDyn documentation
simArmConfigdefines the current position of the robot arm for the RBDyn library, see RBDyn documentation
debugif empty string, no effect, if any other string a trace of the joint angles will be printed to std::cout
Todo:
TODO(ahundt) add torque information

Definition at line 106 of file InverseKinematicsVrepPlugin.hpp.

§ SetVRepArmFromRBDyn()

void grl::vrep::SetVRepArmFromRBDyn ( const std::vector< std::string > &  vrepJointNames,
const std::vector< int > &  vrepJointHandles,
const rbd::MultiBody &  simArmMultiBody,
const rbd::MultiBodyConfig &  simArmConfig,
std::string  debug = "" 
)

Sets the VREP simulation joint positions from the RBDyn configuration

Parameters
vrepJointNamesthe name of each joint, order must match vrepJointHandles
vrepJointHandlesthe V-REP simulation object handle for each joint
rbdJointNamesthe names of all the simArmMultiBody joints, which typically has more joints than the VREP list.
simArmMultiBodydefines the structure of the robot arm for the RBDyn library, see RBDyn documentation
simArmConfigdefines the current position of the robot arm for the RBDyn library, see RBDyn documentation
debugif empty string, no effect, if any other string a trace of the joint angles will be printed to std::cout
Todo:
TODO(ahundt) add torque information

Definition at line 64 of file InverseKinematicsVrepPlugin.hpp.