Commit 6aa404f7 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

Merge branch 'master' into devel

parents c79c20bd 36ed668b
Pipeline #2966 failed with stage
in 49 seconds
......@@ -106,6 +106,14 @@ IF(TALOS_DATA_FOUND)
)
ENDFOREACH(py_filename)
ENDIF(PYRENE_MOTIONS_FOUND)
INSTALL(FILES
${PROJECT_SOURCE_DIR}/tests/test.py
${PROJECT_SOURCE_DIR}/tests/appli.py
DESTINATION
${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/tests
)
ENDIF(TALOS_DATA_FOUND)
SETUP_PROJECT_FINALIZE()
......
......@@ -80,7 +80,7 @@ class Talos(AbstractHumanoidRobot):
from rospkg import RosPack
rospack = RosPack()
urdfPath = rospack.get_path('talos_data')+"/urdf/talos_reduced_v2.urdf"
urdfPath = rospack.get_path('talos_data')+"/urdf/talos_reduced.urdf"
urdfDir = [rospack.get_path('talos_data')+"/../"]
# Create a wrapper to access the dynamic model provided through an urdf file.
......@@ -104,7 +104,7 @@ class Talos(AbstractHumanoidRobot):
# Initialize device
self.device = device
self.timeStep = self.device.getTimeStep()
self.device.resize(self.dynamic.getDimension())
# TODO For position limit, we remove the first value to get
# a vector of the good size because SoT use euler angles and not
......
......@@ -43,7 +43,9 @@ void SoTTalosController::init()
{
std::cout << "Going through SoTTalosController." << std::endl;
ros::NodeHandle &nh = dynamicgraph::rosInit(false,true);
// rosInit is called here only to initialize ros.
// No spinner is initialized.
ros::NodeHandle &nh = dynamicgraph::rosInit(false,false);
interpreter_ = boost::shared_ptr<dynamicgraph::Interpreter>(
new dynamicgraph::Interpreter (nh));
......
......@@ -59,7 +59,6 @@ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SoTTalosDevice,"DeviceTalos");
SoTTalosDevice::SoTTalosDevice(std::string RobotName):
dgsot::Device(RobotName),
timestep_(TIMESTEP_DEFAULT),
previousState_ (),
baseff_ (),
accelerometerSOUT_("Device(" + RobotName + ")::output(vector)::accelerometer"),
......@@ -74,7 +73,9 @@ SoTTalosDevice::SoTTalosDevice(std::string RobotName):
gyrometer_ (3)
{
RESETDEBUG5();
timestep_=TIMESTEP_DEFAULT;
sotDEBUGIN(25) ;
for( int i=0;i<4;++i ) { withForceSignals[i] = true; }
signalRegistration (accelerometerSOUT_ << gyrometerSOUT_
<< currentsSOUT_
......@@ -102,10 +103,6 @@ SoTTalosDevice::SoTTalosDevice(std::string RobotName):
makeCommandVoid1((Device&)*this,
&Device::increment, docstring));
addCommand("getTimeStep",
makeDirectGetter (*this, &this->timestep_,
docDirectGetter ("Time step", "double")));
sotDEBUGOUT(25);
}
......@@ -114,6 +111,7 @@ SoTTalosDevice::~SoTTalosDevice()
void SoTTalosDevice::setSensorsForce(map<string,dgsot::SensorValues> &SensorsIn, int t)
{
int map_sot_2_urdf[4] = { 2, 0, 3, 1};
sotDEBUGIN(15);
map<string,dgsot::SensorValues>::iterator it;
it = SensorsIn.find("forces");
......@@ -125,9 +123,10 @@ void SoTTalosDevice::setSensorsForce(map<string,dgsot::SensorValues> &SensorsIn,
for(int i=0;i<4;++i)
{
sotDEBUG(15) << "Force sensor " << i << std::endl;
int idx_sensor = map_sot_2_urdf[i];
for(int j=0;j<6;++j)
{
dgforces_(j) = forcesIn[i*6+j];
dgforces_(j) = forcesIn[idx_sensor*6+j];
sotDEBUG(15) << "Force value " << j << ":" << dgforces_(j) << std::endl;
}
forcesSOUT[i]->setConstant(dgforces_);
......
......@@ -60,9 +60,6 @@ dgsot::Device
protected:
/// \brief Current integration step.
double timestep_;
/// \brief Previous robot configuration.
dg::Vector previousState_;
......
#from dynamic_graph.sot.core.meta_task_posture import MetaTaskKinePosture
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph.sot.tools import SimpleSeqPlay
from numpy import eye
from dynamic_graph.sot.core import Task, FeatureGeneric, GainAdaptive
from dynamic_graph.sot.core.meta_tasks import setGain
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from numpy import identity, hstack, zeros
# Create the posture task
task_name = "posture_task"
taskPosture = Task(task_name)
taskPosture.dyn = robot.dynamic
taskPosture.feature = FeatureGeneric('feature_'+task_name)
taskPosture.featureDes = FeatureGeneric('feature_des_'+task_name)
taskPosture.gain = GainAdaptive("gain_"+task_name)
robotDim = robot.dynamic.getDimension()
first_6 = zeros((32,6))
other_dof = identity(robotDim-6)
jacobian_posture = hstack([first_6, other_dof])
taskPosture.feature.jacobianIN.value = matrixToTuple( jacobian_posture )
taskPosture.feature.setReference(taskPosture.featureDes.name)
taskPosture.add(taskPosture.feature.name)
# Create the sequence player
aSimpleSeqPlay = SimpleSeqPlay('aSimpleSeqPlay')
aSimpleSeqPlay.load("/home/ostasse/devel-src/robotpkg-test3/install/share/pyrene-motions/identification/OEM_arms_60s_500Hz")
aSimpleSeqPlay.setTimeToStart(10.0)
# Connects the sequence player to the posture task
from dynamic_graph.sot.core import Selec_of_vector
from dynamic_graph import plug
plug(aSimpleSeqPlay.posture, taskPosture.featureDes.errorIN)
# Connects the dynamics to the current feature of the posture task
getPostureValue = Selec_of_vector("current_posture")
getPostureValue.selec(6,robotDim)
plug(robot.dynamic.position, getPostureValue.sin)
plug(getPostureValue.sout, taskPosture.feature.errorIN)
plug(getPostureValue.sout, aSimpleSeqPlay.currentPosture)
# Set the gain of the posture task
setGain(taskPosture.gain,(4.9,0.9,0.01,0.9))
plug(taskPosture.gain.gain, taskPosture.controlGain)
plug(taskPosture.error, taskPosture.gain.error)
# Create the solver
from dynamic_graph.sot.core import SOT
sot = SOT('sot')
sot.setSize(robot.dynamic.getDimension())
plug(sot.control,robot.device.control)
taskPosture.featureDes.errorIN.recompute(0)
# Push the posture task in the solver
......@@ -34,6 +34,13 @@ sot = SOT('sot')
sot.setSize(robot.dynamic.getDimension())
plug(sot.control,robot.device.control)
from dynamic_graph.ros import RosPublish
ros_publish_state = RosPublish ("ros_publish_state")
ros_publish_state.add ("vector", "state", "/sot_control/state")
from dynamic_graph import plug
plug (robot.device.state, ros_publish_state.state)
robot.device.after.addDownsampledSignal ("ros_publish_state.trigger", 100)
#target = (0.5,-0.2,1.0)
#addRobotViewer(robot, small=False)
......
#!/usr/bin/python
import sys
import rospy
from std_srvs.srv import *
from dynamic_graph_bridge.srv import *
from dynamic_graph_bridge_msgs.srv import *
def launchScript(code,title,description = ""):
raw_input(title+': '+description)
rospy.loginfo(title)
rospy.loginfo(code)
for line in code:
if line != '' and line[0] != '#':
print line
answer = runCommandClient(str(line))
rospy.logdebug(answer)
print answer
rospy.loginfo("...done with "+title)
# Waiting for services
try:
rospy.loginfo("Waiting for run_command")
rospy.wait_for_service('/run_command')
rospy.loginfo("...ok")
rospy.loginfo("Waiting for start_dynamic_graph")
rospy.wait_for_service('/start_dynamic_graph')
rospy.loginfo("...ok")
runCommandClient = rospy.ServiceProxy('run_command', RunCommand)
runCommandStartDynamicGraph = rospy.ServiceProxy('start_dynamic_graph', Empty)
initCode = open( "servooff.py", "r").read().split("\n")
rospy.loginfo("Stack of Tasks launched")
launchScript(initCode,'Publishing robotState_ros')
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s" % e)
from dynamic_graph import plug
from dynamic_graph.ros import RosPublish
robot.ros=RosPublish('rosPublish')
robot.ros.add('vector','robotState_ros','robotState')
plug(robot.device.robotState,robot.ros.signal('robotState_ros'))
robot.device.after.addDownsampledSignal('rosPublish.trigger',1);
Markdown is supported
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