search

robotology-playground / WBI-Toolbox

Simulink Toolbox for rapid prototyping of Whole Body Robot Controllers
2 stars 2 forks source link

problem in DoF of the robot #73

Closed azadm closed 9 years ago

azadm commented 9 years ago

Description: I changed robot_fixed to false in $CODYCO_SUPERBUILD_DIR/install/share/codyco/contexts/wholeBodyInterfaceToolbox/wholeBodyInterfaceToolbox.ini but I still get an error in Matlab if I define ROBOT_DOF = 25 and it works with ROBOT_DOF = 10, no matter which model I put in Gazebo.

Further Information:

jeljaik commented 9 years ago

Thanks @azadm Are you using an existing Simulink model? If so, which one? Also, when launching Matlab from terminal, can you paste all the info displayed in the terminal? From those starting with ||

azadm commented 9 years ago

No. I am just trying to read the joint angles. So the model that I am using consists of two blocks which are the Joint Angles and Display blocks. I have iCub (feet fixed) in Gazebo. I define these parameters in matlab:

>> Ts = 0.01;
>> localName = 'simulink';
>> robotName = 'icubGazeboSim';
>> ROBOT_DOF = 25;

Here is the error that I get from matlab:

Error in port widths or dimensions. Input port 2 of 'untitled/Joint Angles/S-Function' is a one dimensional vector with 10 elements.
Error in port widths or dimensions. Output port 1 of 'untitled/Joint Angles/Input Param. for forwardKinematics and computeJacobians 1' is a one dimensional vector with 25 elements.

and this is what I see in the terminal:

mdlInitializeSize >> BLOCK TYPE IS: 0
mdlInitializeSizes >> wbiConfFileName yarpWholeBodyInterface.ini 
mdlInitializeSizes >> wbiConfFileFullPath is: /Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim/yarpWholeBodyInterface.ini
mdlInitializeSizes >> List of properties for yarpWholeBodyInterface was interpreted correctly
mdlInitializeSizes >> ROBOT_DOF: 10 
mdlInitializeSizes >> Options set
jeljaik commented 9 years ago

Ok, @azadm Once more, I need the full log please ... I should see stuff like this in these messages.

[...]
||| checking [/home/jorhabib/.config/yarp/contexts/wholeBodyInterfaceToolbox] (context YARP_CONFIG_HOME)
||| checking [/home/jorhabib/.local/share/yarp/contexts/wholeBodyInterfaceToolbox] (context YARP_DATA_HOME)
||| checking [/etc/xdg/xdg-ubuntu/yarp/contexts/wholeBodyInterfaceToolbox] (context YARP_CONFIG_DIRS)
||| checking [/usr/share/upstart/xdg/yarp/contexts/wholeBodyInterfaceToolbox] (context YARP_CONFIG_DIRS)
||| checking [/etc/xdg/yarp/contexts/wholeBodyInterfaceToolbox] (context YARP_CONFIG_DIRS)
||| checking [contexts/wholeBodyInterfaceToolbox] (context YARP_DATA_DIRS)
||| checking [/home/jorhabib/Software/codyco-superbuild/build/install/share/codyco/contexts/wholeBodyInterfaceToolbox] (context YARP_DATA_DIRS)
||| found /home/jorhabib/Software/codyco-superbuild/build/install/share/codyco/contexts/wholeBodyInterfaceToolbox
||| checking [/home/jorhabib/Software/yarp/build/share/contexts/wholeBodyInterfaceToolbox] (context YARP_DATA_DIRS)
||| checking [/home/jorhabib/Software/codyco-superbuild/build/install/share/codyco/contexts/wholeBodyInterfaceToolbox/wholeBodyInterfaceToolbox.ini] (context)
||| found /home/jorhabib/Software/codyco-superbuild/build/install/share/codyco/contexts/wholeBodyInterfaceToolbox/wholeBodyInterfaceToolbox.ini
||| finding file [yarpWholeBodyInterface.ini]
||| checking [/home/jorhabib/Documents/MATLAB/yarpWholeBodyInterface.ini] (pwd)
[...]
azadm commented 9 years ago

I know @jeljaik but that was all I get in the matlab terminal. The lines you mentioned with ||| appear when I define ROBOT_DOF = 10; which are:

mdlStart >> STARTED
mdlStart >> Publicly stating that a new child has been born: 1 
mdlStart >> The string being passed for robotName is - icubGazeboSim
mdlStart >> Block type mask parameter: 0.000000
mdlStart >> The link passed for parametric blocks is: na
mdlStart >> YARP is up and running!!
robotStatus::robotStatus >> ROBOTSTATUS instantiated 1 times
mdlStart >> An object robot of type wholeBodyInterface has been created
mdlStart >> About to configure robot 
robotStatus::robotConfig >> Configuring...
||| clearing context
||| adding context [wholeBodyInterfaceToolbox]
||| configuring
||| no policy found
||| default config file specified as wholeBodyInterfaceToolbox.ini
||| checking [/Users/azadm/Documents/codyco-superbuild/main/WBIToolbox/controllers/wholeBodyInterfaceToolbox.ini] (pwd)
||| checking [/Users/azadm/Library/Application Support/yarp/config/robots/icubGazeboSim] (robot YARP_CONFIG_HOME)
||| checking [/Users/azadm/Library/Application Support/yarp/robots/icubGazeboSim] (robot YARP_DATA_HOME)
||| checking [/Library/Preferences/yarp/robots/icubGazeboSim] (robot YARP_CONFIG_DIRS)
||| checking [/etc/yarp/robots/icubGazeboSim] (robot YARP_CONFIG_DIRS)
||| checking [/Users/azadm/Documents/yarp/build/share/yarp/robots/icubGazeboSim] (robot YARP_DATA_DIRS)
||| checking [/Users/azadm/Documents/icub-main/build/share/iCub/robots/icubGazeboSim] (robot YARP_DATA_DIRS)
||| checking [/Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim] (robot YARP_DATA_DIRS)
||| found /Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim
||| checking [/Users/azadm/Documents/yarp/build/share/yarp/config/path.d] (robot path.d YARP_DATA_DIRS)
||| checking [/Users/azadm/Documents/icub-main/build/share/iCub/config/path.d] (robot path.d YARP_DATA_DIRS)
||| checking [/Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/config/path.d] (robot path.d YARP_DATA_DIRS)
||| checking [/Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim/wholeBodyInterfaceToolbox.ini] (robot)
||| checking [/Users/azadm/Library/Application Support/yarp/config/contexts/wholeBodyInterfaceToolbox] (context YARP_CONFIG_HOME)
||| checking [/Users/azadm/Library/Application Support/yarp/contexts/wholeBodyInterfaceToolbox] (context YARP_DATA_HOME)
||| checking [/Library/Preferences/yarp/contexts/wholeBodyInterfaceToolbox] (context YARP_CONFIG_DIRS)
||| checking [/etc/yarp/contexts/wholeBodyInterfaceToolbox] (context YARP_CONFIG_DIRS)
||| checking [/Users/azadm/Documents/yarp/build/share/yarp/contexts/wholeBodyInterfaceToolbox] (context YARP_DATA_DIRS)
||| checking [/Users/azadm/Documents/icub-main/build/share/iCub/contexts/wholeBodyInterfaceToolbox] (context YARP_DATA_DIRS)
||| checking [/Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/contexts/wholeBodyInterfaceToolbox] (context YARP_DATA_DIRS)
||| found /Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/contexts/wholeBodyInterfaceToolbox
||| checking [/Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/contexts/wholeBodyInterfaceToolbox/wholeBodyInterfaceToolbox.ini] (context)
||| found /Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/contexts/wholeBodyInterfaceToolbox/wholeBodyInterfaceToolbox.ini
||| finding file [yarpWholeBodyInterface.ini]
||| checking [/Users/azadm/Documents/codyco-superbuild/main/WBIToolbox/controllers/yarpWholeBodyInterface.ini] (pwd)
||| checking [/Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim/yarpWholeBodyInterface.ini] (robot)
||| found /Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim/yarpWholeBodyInterface.ini
robotStatus::robotConfig >> wbiConfFileFullPath is: /Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim/yarpWholeBodyInterface.ini
[robotStatus::robotConfig] List of properties for yarpWholeBodyInterface was interpreted correctly
[robotStatus::robotConfig] robot_fixed is false!
[robotStatus::robotConfig] After reading from config file, params are 
[robotStatus::robotConfig] robot name:            icubGazeboSim 
[robotStatus::robotConfig] local name:            simulink 
[robotStatus::robotConfig] world reference frame: l_sole 
[robotStatus::robotConfig] WBI ID List:           ROBOT_TORQUE_CONTROL_JOINTS 
RemoteControlBoard is ENABLING the writeStrict option for all commands
yarp: Port /simulinkactuator/left_arm/rpc:o active at tcp://147.188.200.113:10034
yarp: Port /simulinkactuator/left_arm/command:o active at tcp://147.188.200.113:10035
yarp: Port /simulinkactuator/left_arm/state:i active at tcp://147.188.200.113:10036
yarp: Sending output from /simulinkactuator/left_arm/rpc:o to /icubGazeboSim/left_arm/rpc:i using tcp
yarp: Sending output from /simulinkactuator/left_arm/command:o to /icubGazeboSim/left_arm/command:i using tcp
yarp: Receiving input from /icubGazeboSim/left_arm/state:o to /simulinkactuator/left_arm/state:i using udp
[INFO]created device <remote_controlboard>. See C++ class RemoteControlBoard for documentation.
RemoteControlBoard is ENABLING the writeStrict option for all commands
yarp: Port /simulinkactuator/right_arm/rpc:o active at tcp://147.188.200.113:10037
yarp: Port /simulinkactuator/right_arm/command:o active at tcp://147.188.200.113:10038
yarp: Port /simulinkactuator/right_arm/state:i active at tcp://147.188.200.113:10039
yarp: Sending output from /simulinkactuator/right_arm/rpc:o to /icubGazeboSim/right_arm/rpc:i using tcp
yarp: Sending output from /simulinkactuator/right_arm/command:o to /icubGazeboSim/right_arm/command:i using tcp
yarp: Receiving input from /icubGazeboSim/right_arm/state:o to /simulinkactuator/right_arm/state:i using udp
[INFO]created device <remote_controlboard>. See C++ class RemoteControlBoard for documentation.
[INFO] Link root_link had 3 children
[INFO] Link l_hip_1 had 1 children
[INFO] Link l_hip_2 had 1 children
[INFO] Link l_hip_3 had 1 children
[INFO] Link l_thigh had 1 children
[INFO] Link l_shank had 1 children
[INFO] Link l_ankle_1 had 1 children
[INFO] Link l_upper_foot had 1 children
[INFO] Link l_foot had 1 children
[INFO] Link l_sole had 0 children
[INFO] Link r_hip_1 had 1 children
[INFO] Link r_hip_2 had 1 children
[INFO] Link r_hip_3 had 1 children
[INFO] Link r_thigh had 1 children
[INFO] Link r_shank had 1 children
[INFO] Link r_ankle_1 had 1 children
[INFO] Link r_upper_foot had 1 children
[INFO] Link r_foot had 1 children
[INFO] Link r_sole had 0 children
[INFO] Link lap_belt_1 had 1 children
[INFO] Link lap_belt_2 had 1 children
[INFO] Link chest had 4 children
[INFO] Link l_shoulder_1 had 1 children
[INFO] Link l_shoulder_2 had 1 children
[INFO] Link l_upper_arm had 1 children
[INFO] Link l_arm had 1 children
[INFO] Link l_elbow_1 had 1 children
[INFO] Link l_forearm had 1 children
[INFO] Link l_wrist_1 had 1 children
[INFO] Link l_hand had 1 children
[INFO] Link l_gripper had 0 children
[INFO] Link neck_1 had 1 children
[INFO] Link neck_2 had 1 children
[INFO] Link head had 1 children
[INFO] Link imu_frame had 0 children
[INFO] Link r_shoulder_1 had 1 children
[INFO] Link r_shoulder_2 had 1 children
[INFO] Link r_upper_arm had 1 children
[INFO] Link r_arm had 1 children
[INFO] Link r_elbow_1 had 1 children
[INFO] Link r_forearm had 1 children
[INFO] Link r_wrist_1 had 1 children
[INFO] Link r_hand had 1 children
[INFO] Link r_gripper had 0 children
[INFO] Link torso had 0 children
[INFO]

Found world reference frame mention in yarpConfig. Setting as  r_sole 

[INFO]Reference link set as world was  9 
[INFO], now it is set to  18 

[WARN] yarpWholeBodyStates::loadCouplingsFromConfigurationFile : WBI_YARP_JOINTS_MOTOR_KINEMATIC_COUPLINGS group not found in configuration file
RemoteControlBoard is ENABLING the writeStrict option for all commands
yarp: Port /simulinkstate/left_arm/rpc:o active at tcp://147.188.200.113:10040
yarp: Port /simulinkstate/left_arm/command:o active at tcp://147.188.200.113:10041
yarp: Port /simulinkstate/left_arm/state:i active at tcp://147.188.200.113:10042
yarp: Sending output from /simulinkstate/left_arm/rpc:o to /icubGazeboSim/left_arm/rpc:i using tcp
yarp: Sending output from /simulinkstate/left_arm/command:o to /icubGazeboSim/left_arm/command:i using tcp
yarp: Receiving input from /icubGazeboSim/left_arm/state:o to /simulinkstate/left_arm/state:i using udp
[INFO]created device <remote_controlboard>. See C++ class RemoteControlBoard for documentation.
RemoteControlBoard is ENABLING the writeStrict option for all commands
yarp: Port /simulinkstate/right_arm/rpc:o active at tcp://147.188.200.113:10043
yarp: Port /simulinkstate/right_arm/command:o active at tcp://147.188.200.113:10044
yarp: Port /simulinkstate/right_arm/state:i active at tcp://147.188.200.113:10045
yarp: Sending output from /simulinkstate/right_arm/rpc:o to /icubGazeboSim/right_arm/rpc:i using tcp
yarp: Sending output from /simulinkstate/right_arm/command:o to /icubGazeboSim/right_arm/command:i using tcp
yarp: Receiving input from /icubGazeboSim/right_arm/state:o to /simulinkstate/right_arm/state:i using udp
[INFO]created device <remote_controlboard>. See C++ class RemoteControlBoard for documentation.
[DEBUG] yarpWholeBodyStates correctly initialized 
[robotStatus::robotConfig] Whole Body Interface correctly initialized
[robotStatus::robotConfig] About to set control mode
RemoteControlBoard is ENABLING the writeStrict option for all commands
yarp: Port /model/left_arm/rpc:o active at tcp://147.188.200.113:10046
yarp: Port /model/left_arm/command:o active at tcp://147.188.200.113:10047
yarp: Port /model/left_arm/state:i active at tcp://147.188.200.113:10048
yarp: Sending output from /model/left_arm/rpc:o to /icubGazeboSim/left_arm/rpc:i using tcp
yarp: Sending output from /model/left_arm/command:o to /icubGazeboSim/left_arm/command:i using tcp
yarp: Receiving input from /icubGazeboSim/left_arm/state:o to /model/left_arm/state:i using udp
[INFO]created device <remote_controlboard>. See C++ class RemoteControlBoard for documentation.
RemoteControlBoard is ENABLING the writeStrict option for all commands
yarp: Port /model/right_arm/rpc:o active at tcp://147.188.200.113:10049
yarp: Port /model/right_arm/command:o active at tcp://147.188.200.113:10050
yarp: Port /model/right_arm/state:i active at tcp://147.188.200.113:10051
yarp: Sending output from /model/right_arm/rpc:o to /icubGazeboSim/right_arm/rpc:i using tcp
yarp: Sending output from /model/right_arm/command:o to /icubGazeboSim/right_arm/command:i using tcp
yarp: Receiving input from /icubGazeboSim/right_arm/state:o to /model/right_arm/state:i using udp
[INFO]created device <remote_controlboard>. See C++ class RemoteControlBoard for documentation.
yarp: Removing output from /model/left_arm/rpc:o to /icubGazeboSim/left_arm/rpc:i
yarp: Removing output from /model/left_arm/command:o to /icubGazeboSim/left_arm/command:i
yarp: Removing input from /icubGazeboSim/left_arm/state:o to /model/left_arm/state:i
yarp: Removing output from /model/right_arm/rpc:o to /icubGazeboSim/right_arm/rpc:i
yarp: Removing output from /model/right_arm/command:o to /icubGazeboSim/right_arm/command:i
yarp: Removing input from /icubGazeboSim/right_arm/state:o to /model/right_arm/state:i
mdlStart >> Succesfully exited robotConfig.
[DEBUGGING] Before resizing jacobianMAtrix
[DEBUGGING] ROBOT_DOF is: 10
[DEBUGGING] After resizing jacobianMatrix
robotStatus::robotInit >> Finished robotInit
mdlStart >> Succesfully exited robotInit.
mdlStart >> FINISHED

I AM GETTING UPDATED!!! 
mdlTerminate >> Inside robot object 0x7fa87cc92c00 
robotStatus::~robotStatus >> wbInterface in destructor: 0x7fa880b11660 
yarp: Removing output from /simulinkactuator/left_arm/rpc:o to /icubGazeboSim/left_arm/rpc:i
yarp: Removing output from /simulinkactuator/left_arm/command:o to /icubGazeboSim/left_arm/command:i
yarp: Removing input from /icubGazeboSim/left_arm/state:o to /simulinkactuator/left_arm/state:i
yarp: Removing output from /simulinkactuator/right_arm/rpc:o to /icubGazeboSim/right_arm/rpc:i
yarp: Removing output from /simulinkactuator/right_arm/command:o to /icubGazeboSim/right_arm/command:i
yarp: Removing input from /icubGazeboSim/right_arm/state:o to /simulinkactuator/right_arm/state:i
yarp: Removing output from /simulinkstate/left_arm/rpc:o to /icubGazeboSim/left_arm/rpc:i
yarp: Removing output from /simulinkstate/left_arm/command:o to /icubGazeboSim/left_arm/command:i
yarp: Removing input from /icubGazeboSim/left_arm/state:o to /simulinkstate/left_arm/state:i
yarp: Removing output from /simulinkstate/right_arm/rpc:o to /icubGazeboSim/right_arm/rpc:i
yarp: Removing output from /simulinkstate/right_arm/command:o to /icubGazeboSim/right_arm/command:i
yarp: Removing input from /icubGazeboSim/right_arm/state:o to /simulinkstate/right_arm/state:i
robotStatus::~robotStatus >> wbInterface has been closed and deleted correctly. -1 to go 
Left mdlTerminate
jeljaik commented 9 years ago

By Terminal I mean the Ubuntu/Linux Terminal :) ... Matlab's is called Command Window but this is exactly what I want. Let me take a look.

azadm commented 9 years ago

Yes, I know. My answer is still the same. I DO NOT get these lines in the TERMINAL when I define ROBOT_DOF = 25;. The one before the last is all I get in the TERMINAL that Matlab is running and the above lines appear in the TERMINAL when I define ROBOT_DOF = 10;.

jeljaik commented 9 years ago

oh ok ok, that means that you have updated to the latest version of the Toolbox (In the latest commits we redirected everything to Matlab's diagnostics terminal). When you first compiled the issue we saw you were in a different revision, that's why I thought you were getting confused with the terminal :) In any case, can you post also the content of: /Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim/yarpWholeBodyInterface.ini

azadm commented 9 years ago

Sure.

robot                      icubGazeboSim
urdf                       model.urdf
getLimitsFromControlBoard

#Original information extracted from http://eris.liralab.it/wiki/ICub_joints
#Every "actuated" joint in the wbi is mapped to an axes of a yarp controlboard
#torso
#left_leg
#right_leg

#joints
#IMU
#FTS
[WBI_YARP_JOINTS]
torso_yaw        = (torso,0)
torso_roll       = (torso,1)
torso_pitch      = (torso,2)

neck_pitch       = (head,0)
neck_roll        = (head,1)
neck_yaw         = (head,2)

l_shoulder_pitch = (left_arm,0)
l_shoulder_roll  = (left_arm,1)
l_shoulder_yaw   = (left_arm,2)
l_elbow          = (left_arm,3)
l_wrist_prosup   = (left_arm,4)
l_wrist_pitch    = (left_arm,5)
l_wrist_yaw      = (left_arm,6)

r_shoulder_pitch = (right_arm,0)
r_shoulder_roll  = (right_arm,1)
r_shoulder_yaw   = (right_arm,2)
r_elbow          = (right_arm,3)
r_wrist_prosup   = (right_arm,4)
r_wrist_pitch    = (right_arm,5)
r_wrist_yaw      = (right_arm,6)

l_hip_pitch      = (left_leg,0)
l_hip_roll       = (left_leg,1)
l_hip_yaw        = (left_leg,2)
l_knee           = (left_leg,3)
l_ankle_pitch    = (left_leg,4)
l_ankle_roll     = (left_leg,5)

r_hip_pitch      = (right_leg,0)
r_hip_roll       = (right_leg,1)
r_hip_yaw        = (right_leg,2)
r_knee           = (right_leg,3)
r_ankle_pitch    = (right_leg,4)
r_ankle_roll     = (right_leg,5)

[WBI_YARP_IMU_PORTS]
imu_frame /inertial

[WBI_YARP_FT_PORTS]
l_leg_ft_sensor /left_leg/analog:o
l_foot_ft_sensor /left_foot/analog:o
r_leg_ft_sensor /right_leg/analog:o
r_foot_ft_sensor /right_foot/analog:o

[WBI_ID_LISTS]
# Joint lists
ROBOT_HEAD_JOINTS           = (neck_pitch, neck_roll, neck_yaw)

ROBOT_LEFT_ARM_JOINTS       = (l_shoulder_pitch, l_shoulder_roll, l_shoulder_yaw, l_elbow, l_wrist_prosup)
ROBOT_RIGHT_ARM_JOINTS      = (r_shoulder_pitch, r_shoulder_roll, r_shoulder_yaw, r_elbow, r_wrist_prosup)

ROBOT_LEFT_ARM_DYNAMIC_MODEL_JOINTS = (ROBOT_LEFT_ARM_JOINTS,l_wrist_pitch,l_wrist_yaw)
ROBOT_RIGHT_ARM_DYNAMIC_MODEL_JOINTS = (ROBOT_RIGHT_ARM_JOINTS,r_wrist_pitch,r_wrist_yaw)

ROBOT_DYNAMIC_MODEL_JOINTS  = (ROBOT_HEAD_JOINTS, ROBOT_LEFT_ARM_DYNAMIC_MODEL_JOINTS, ROBOT_RIGHT_ARM_DYNAMIC_MODEL_JOINTS)

ROBOT_MAIN_JOINTS           = (ROBOT_LEFT_ARM_JOINTS, ROBOT_RIGHT_ARM_JOINTS)

ROBOT_TORQUE_CONTROL_JOINTS = (ROBOT_LEFT_ARM_JOINTS, ROBOT_RIGHT_ARM_JOINTS)

ROBOT_TORQUE_SINGLE_JOINTS = (l_hip_pitch)

ROBOT_JOINTS_WBITOOLBOX     = (ROBOT_TORQUE_CONTROL_JOINTS)
ROBOT_MEX_WBI_TOOLBOX       = (ROBOT_TORQUE_CONTROL_JOINTS)

# List of imus of the robot
ROBOT_MAIN_IMUS             = (imu_frame)

# List of 6-axes ft sensors of the robot
ROBOT_LEFT_ARM_FTS          = (l_arm_ft_sensor)
ROBOT_RIGHT_ARM_FTS         = (r_arm_ft_sensor)
ROBOT_MAIN_FTS              = (ROBOT_LEFT_ARM_FTS,ROBOT_RIGHT_ARM_FTS)

ROBOT_TORQUE_CONTROL_JOINTS_WITHOUT_PRONOSUP = (l_shoulder_pitch, l_shoulder_roll, l_shoulder_yaw, l_elbow, r_shoulder_pitch, r_shoulder_roll, r_shoulder_yaw, r_elbow)

[WBI_STATE_OPTIONS]
WORLD_REFERENCE_FRAME r_sole
jeljaik commented 9 years ago

I see the problem @azadm The line ROBOT_TORQUE_CONTROL_JOINTS = (ROBOT_LEFT_ARM_JOINTS, ROBOT_RIGHT_ARM_JOINTS) should have more links included. Having only the left arm and right arm gives you 10 DOF. Four days ago it was wrongly modified in our lab. Sorry for the inconvenience. We just realized now with your issue. Thanks!

I have updated the configuration file of the yarpWholeBodyInterface in: https://github.com/robotology-playground/yarp-wholebodyinterface/commit/990ba15787e5e29a9870604bb1a84beff4a97fa3

So, now please do:

cd /Users/azadm/Documents/codyco-superbuild/libraries/yarpWholeBodyInterface
git pull origin master
make
make install
DanielePucci commented 9 years ago

Deeply sorry @azadm: unlucky copy and paste. It was my fault!

jeljaik commented 9 years ago

Please let us know if this solves your issue

azadm commented 9 years ago

Thanks @jeljaik and @DanielePucci . Now I get

mdlInitializeSizes >> ROBOT_DOF: 13

in the terminal. Is there anything else that needs to be changed or I am doing something wrong? Now my yarpWholeBodyInterface.ini looks like this:

robot                      icubGazeboSim
urdf                       model.urdf
getLimitsFromControlBoard

#Original information extracted from http://eris.liralab.it/wiki/ICub_joints
#Every "actuated" joint in the wbi is mapped to an axes of a yarp controlboard
#torso
#left_leg
#right_leg

#joints
#IMU
#FTS
[WBI_YARP_JOINTS]
torso_yaw        = (torso,0)
torso_roll       = (torso,1)
torso_pitch      = (torso,2)

neck_pitch       = (head,0)
neck_roll        = (head,1)
neck_yaw         = (head,2)

l_shoulder_pitch = (left_arm,0)
l_shoulder_roll  = (left_arm,1)
l_shoulder_yaw   = (left_arm,2)
l_elbow          = (left_arm,3)
l_wrist_prosup   = (left_arm,4)
l_wrist_pitch    = (left_arm,5)
l_wrist_yaw      = (left_arm,6)

r_shoulder_pitch = (right_arm,0)
r_shoulder_roll  = (right_arm,1)
r_shoulder_yaw   = (right_arm,2)
r_elbow          = (right_arm,3)
r_wrist_prosup   = (right_arm,4)
r_wrist_pitch    = (right_arm,5)
r_wrist_yaw      = (right_arm,6)

l_hip_pitch      = (left_leg,0)
l_hip_roll       = (left_leg,1)
l_hip_yaw        = (left_leg,2)
l_knee           = (left_leg,3)
l_ankle_pitch    = (left_leg,4)
l_ankle_roll     = (left_leg,5)

r_hip_pitch      = (right_leg,0)
r_hip_roll       = (right_leg,1)
r_hip_yaw        = (right_leg,2)
r_knee           = (right_leg,3)
r_ankle_pitch    = (right_leg,4)
r_ankle_roll     = (right_leg,5)

[WBI_YARP_IMU_PORTS]
imu_frame /inertial

[WBI_YARP_FT_PORTS]
l_leg_ft_sensor /left_leg/analog:o
l_foot_ft_sensor /left_foot/analog:o
r_leg_ft_sensor /right_leg/analog:o
r_foot_ft_sensor /right_foot/analog:o

[WBI_ID_LISTS]
# Joint lists
ROBOT_HEAD_JOINTS           = (neck_pitch, neck_roll, neck_yaw)

ROBOT_LEFT_ARM_JOINTS       = (l_shoulder_pitch, l_shoulder_roll, l_shoulder_yaw, l_elbow, l_wrist_prosup)
ROBOT_RIGHT_ARM_JOINTS      = (r_shoulder_pitch, r_shoulder_roll, r_shoulder_yaw, r_elbow, r_wrist_prosup)

ROBOT_LEFT_ARM_DYNAMIC_MODEL_JOINTS = (ROBOT_LEFT_ARM_JOINTS,l_wrist_pitch,l_wrist_yaw)
ROBOT_RIGHT_ARM_DYNAMIC_MODEL_JOINTS = (ROBOT_RIGHT_ARM_JOINTS,r_wrist_pitch,r_wrist_yaw)

ROBOT_DYNAMIC_MODEL_JOINTS  = (ROBOT_HEAD_JOINTS, ROBOT_LEFT_ARM_DYNAMIC_MODEL_JOINTS, ROBOT_RIGHT_ARM_DYNAMIC_MODEL_JOINTS)

ROBOT_MAIN_JOINTS           = (ROBOT_LEFT_ARM_JOINTS, ROBOT_RIGHT_ARM_JOINTS)

ROBOT_TORQUE_CONTROL_JOINTS = (ROBOT_TORSO_JOINTS, ROBOT_LEFT_ARM_JOINTS, ROBOT_RIGHT_ARM_JOINTS, ROBOT_LEFT_LEG_JOINTS, ROBOT_RIGHT_LEG_JOINTS)

ROBOT_TORQUE_SINGLE_JOINTS = (l_hip_pitch)

ROBOT_JOINTS_WBITOOLBOX     = (ROBOT_TORQUE_CONTROL_JOINTS)
ROBOT_MEX_WBI_TOOLBOX       = (ROBOT_TORQUE_CONTROL_JOINTS)

# List of imus of the robot
ROBOT_MAIN_IMUS             = (imu_frame)

# List of 6-axes ft sensors of the robot
ROBOT_LEFT_ARM_FTS          = (l_arm_ft_sensor)
ROBOT_RIGHT_ARM_FTS         = (r_arm_ft_sensor)
ROBOT_MAIN_FTS              = (ROBOT_LEFT_ARM_FTS,ROBOT_RIGHT_ARM_FTS)

ROBOT_TORQUE_CONTROL_JOINTS_WITHOUT_PRONOSUP = (l_shoulder_pitch, l_shoulder_roll, l_shoulder_yaw, l_elbow, r_shoulder_pitch, r_shoulder_roll, r_shoulder_yaw, r_elbow)

[WBI_STATE_OPTIONS]
WORLD_REFERENCE_FRAME r_sole
DanielePucci commented 9 years ago

Mmmm, that's fishy. Can you copy-and-paste the output of the terminal (as you did before)?

azadm commented 9 years ago 
mdlInitializeSize >> BLOCK TYPE IS: 0
mdlInitializeSizes >> wbiConfFileName yarpWholeBodyInterface.ini 
mdlInitializeSizes >> wbiConfFileFullPath is: /Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim/yarpWholeBodyInterface.ini
mdlInitializeSizes >> List of properties for yarpWholeBodyInterface was interpreted correctly
mdlInitializeSizes >> ROBOT_DOF: 13 
mdlInitializeSizes >> Options set

mdlStart >> STARTED
mdlStart >> Publicly stating that a new child has been born: 1 
mdlStart >> The string being passed for robotName is - icubGazeboSim
mdlStart >> Block type mask parameter: 0.000000
mdlStart >> The link passed for parametric blocks is: na
mdlStart >> YARP is up and running!!
robotStatus::robotStatus >> ROBOTSTATUS instantiated 1 times
mdlStart >> An object robot of type wholeBodyInterface has been created
mdlStart >> About to configure robot 
robotStatus::robotConfig >> Configuring...
||| clearing context
||| adding context [wholeBodyInterfaceToolbox]
||| configuring
||| no policy found
||| default config file specified as wholeBodyInterfaceToolbox.ini
||| checking [/Users/azadm/Documents/codyco-superbuild/main/WBIToolbox/controllers/wholeBodyInterfaceToolbox.ini] (pwd)
||| checking [/Users/azadm/Library/Application Support/yarp/config/robots/icubGazeboSim] (robot YARP_CONFIG_HOME)
||| checking [/Users/azadm/Library/Application Support/yarp/robots/icubGazeboSim] (robot YARP_DATA_HOME)
||| checking [/Library/Preferences/yarp/robots/icubGazeboSim] (robot YARP_CONFIG_DIRS)
||| checking [/etc/yarp/robots/icubGazeboSim] (robot YARP_CONFIG_DIRS)
||| checking [/Users/azadm/Documents/yarp/build/share/yarp/robots/icubGazeboSim] (robot YARP_DATA_DIRS)
||| checking [/Users/azadm/Documents/icub-main/build/share/iCub/robots/icubGazeboSim] (robot YARP_DATA_DIRS)
||| checking [/Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim] (robot YARP_DATA_DIRS)
||| found /Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim
||| checking [/Users/azadm/Documents/yarp/build/share/yarp/config/path.d] (robot path.d YARP_DATA_DIRS)
||| checking [/Users/azadm/Documents/icub-main/build/share/iCub/config/path.d] (robot path.d YARP_DATA_DIRS)
||| checking [/Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/config/path.d] (robot path.d YARP_DATA_DIRS)
||| checking [/Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim/wholeBodyInterfaceToolbox.ini] (robot)
||| checking [/Users/azadm/Library/Application Support/yarp/config/contexts/wholeBodyInterfaceToolbox] (context YARP_CONFIG_HOME)
||| checking [/Users/azadm/Library/Application Support/yarp/contexts/wholeBodyInterfaceToolbox] (context YARP_DATA_HOME)
||| checking [/Library/Preferences/yarp/contexts/wholeBodyInterfaceToolbox] (context YARP_CONFIG_DIRS)
||| checking [/etc/yarp/contexts/wholeBodyInterfaceToolbox] (context YARP_CONFIG_DIRS)
||| checking [/Users/azadm/Documents/yarp/build/share/yarp/contexts/wholeBodyInterfaceToolbox] (context YARP_DATA_DIRS)
||| checking [/Users/azadm/Documents/icub-main/build/share/iCub/contexts/wholeBodyInterfaceToolbox] (context YARP_DATA_DIRS)
||| checking [/Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/contexts/wholeBodyInterfaceToolbox] (context YARP_DATA_DIRS)
||| found /Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/contexts/wholeBodyInterfaceToolbox
||| checking [/Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/contexts/wholeBodyInterfaceToolbox/wholeBodyInterfaceToolbox.ini] (context)
||| found /Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/contexts/wholeBodyInterfaceToolbox/wholeBodyInterfaceToolbox.ini
||| finding file [yarpWholeBodyInterface.ini]
||| checking [/Users/azadm/Documents/codyco-superbuild/main/WBIToolbox/controllers/yarpWholeBodyInterface.ini] (pwd)
||| checking [/Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim/yarpWholeBodyInterface.ini] (robot)
||| found /Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim/yarpWholeBodyInterface.ini
robotStatus::robotConfig >> wbiConfFileFullPath is: /Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim/yarpWholeBodyInterface.ini
[robotStatus::robotConfig] List of properties for yarpWholeBodyInterface was interpreted correctly
[robotStatus::robotConfig] robot_fixed is false!
[robotStatus::robotConfig] After reading from config file, params are 
[robotStatus::robotConfig] robot name:            icubGazeboSim 
[robotStatus::robotConfig] local name:            simulink 
[robotStatus::robotConfig] world reference frame: l_sole 
[robotStatus::robotConfig] WBI ID List:           ROBOT_TORQUE_CONTROL_JOINTS 
[ERROR]wholeBodyInterface error: joint  ROBOT_TORSO_JOINTS not found in WBI_YARP_JOINTS section of configuration file  
[ERR] Error while initializing yarpWholeBodyActuators interface.
[ERR] Error while initializing yarpWholeBodyModel interface.
[ERR] Error while initializing yarpWholeBodyStates interface.
ERR [robotStatus::robotConfig] Initializing Whole Body Interface!
mdlTerminate >> Inside robot object 0x7fe2227d0000 
[ERR] robotStatus::setControlMode : Reference speeds could not be set Succesfully
robotStatus::~robotStatus >> wbInterface in destructor: 0x7fe2163460a0 
robotStatus::~robotStatus >> wbInterface has been closed and deleted correctly. -1 to go 
Left mdlTerminate
DanielePucci commented 9 years ago

Ok, now please post the following two files:

/Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/contexts/wholeBodyInterfaceToolbox/wholeBodyInterfaceToolbox.ini

/Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim/yarpWholeBodyInterface.ini

DanielePucci commented 9 years ago

Anyway, this is the problem

[ERROR]wholeBodyInterface error: joint  ROBOT_TORSO_JOINTS not found in WBI_YARP_JOINTS section of configuration file  
[ERR] Error while initializing yarpWholeBodyActuators interface.
[ERR] Error while initializing yarpWholeBodyModel interface.
[ERR] Error while initializing yarpWholeBodyStates interface.
ERR [robotStatus::robotConfig] Initializing Whole Body Interface!
mdlTerminate >> Inside robot object 0x7fe2227d0000 
[ERR] robotStatus::setControlMode : Reference speeds could not be set Succesfully
azadm commented 9 years ago

I already posted the second file and here is the first file:

robot          icubGazeboSim
localName      simulink
worldRefFrame  l_sole
robot_fixed    false
wbi_id_list    ROBOT_TORQUE_CONTROL_JOINTS
wbi_config_file yarpWholeBodyInterface.ini
jeljaik commented 9 years ago

Please do again

cd /Users/azadm/Documents/codyco-superbuild/libraries/yarpWholeBodyInterface
git pull origin master
make
make install

There was yet another wrongly introduced error.

jeljaik commented 9 years ago

For me it's working fine now @azadm. Can you confirm it's the same for you?

azadm commented 9 years ago

Yes. It is working fine now. Thank you @jeljaik and @DanielePucci

DanielePucci commented 9 years ago

You are welcome.