Commit ff60e3af authored by Olivier Stasse's avatar Olivier Stasse

Add position control while waiting for start_dynamic_graph.

parent 2c8f7baa
...@@ -312,6 +312,54 @@ namespace sot_controller ...@@ -312,6 +312,54 @@ namespace sot_controller
return true; return true;
} }
bool RCSotController::
readParamsPositionControlData(ros::NodeHandle &robot_nh)
{
// Read libname
if (robot_nh.hasParam("/sot_controller/position_control_init_pos"))
{
XmlRpc::XmlRpcValue xml_rpc_pci_pose;
robot_nh.getParamCached("/sot_controller/position_control_init_pos",
xml_rpc_pci_pose);
ROS_INFO("/sot_controller/position_control_init_pos: %d %d %d\n",
xml_rpc_pci_pose.getType(),XmlRpc::XmlRpcValue::TypeArray,XmlRpc::XmlRpcValue::TypeStruct);
desired_init_pose_.clear();
for (size_t i=0;i<xml_rpc_pci_pose.size();i++)
{
XmlRpc::XmlRpcValue xml_rpc_pci_el;
ROS_INFO("/sot_controller/position_control_init_pos/ %d: %d %d %d\n",i,
xml_rpc_pci_pose[i].getType(),XmlRpc::XmlRpcValue::TypeArray,XmlRpc::XmlRpcValue::TypeStruct);
if (xml_rpc_pci_pose[i].hasMember("name"))
{
std::string local_joint_name = std::string(xml_rpc_pci_pose[i]["name"]);
if (xml_rpc_pci_pose[i].hasMember("name"))
{
double local_des_pose = double(xml_rpc_pci_pose[i]["des_pos"]);
desired_init_pose_[local_joint_name] = local_des_pose;
}
else
{
ROS_INFO("parameter /sot_controller/position_control_init_pos/%d needs a desired position\n", i );
return false;
}
}
else
{
ROS_INFO("parameter /sot_controller/position_control_init_pos/%d needs a name\n",i );
return false;
}
}
return true;
}
ROS_ERROR("No parameter /sot_controller/position_control_init_pos");
return false;
}
bool RCSotController:: bool RCSotController::
readParamsEffortControlPDMotorControlData(ros::NodeHandle &robot_nh) readParamsEffortControlPDMotorControlData(ros::NodeHandle &robot_nh)
{ {
...@@ -496,7 +544,8 @@ namespace sot_controller ...@@ -496,7 +544,8 @@ namespace sot_controller
if (control_mode_==EFFORT) if (control_mode_==EFFORT)
readParamsEffortControlPDMotorControlData(robot_nh); readParamsEffortControlPDMotorControlData(robot_nh);
else if (control_mode_==POSITION)
readParamsPositionControlData(robot_nh);
return true; return true;
} }
...@@ -623,11 +672,11 @@ namespace sot_controller ...@@ -623,11 +672,11 @@ namespace sot_controller
for(unsigned int idJoint=0;idJoint<joints_.size();idJoint++) for(unsigned int idJoint=0;idJoint<joints_.size();idJoint++)
{ {
DataOneIter_.motor_angle[idJoint] = joints_[idJoint].getPosition(); DataOneIter_.motor_angle[idJoint] = joints_[idJoint].getPosition();
if (!simulation_mode_) //if (!simulation_mode_)
DataOneIter_.joint_angle[idJoint] = joints_[idJoint].getAbsolutePosition(); DataOneIter_.joint_angle[idJoint] = joints_[idJoint].getAbsolutePosition();
DataOneIter_.velocities[idJoint] = joints_[idJoint].getVelocity(); DataOneIter_.velocities[idJoint] = joints_[idJoint].getVelocity();
if (!simulation_mode_) //if (!simulation_mode_)
DataOneIter_.torques[idJoint] = joints_[idJoint].getTorqueSensor(); DataOneIter_.torques[idJoint] = joints_[idJoint].getTorqueSensor();
DataOneIter_.motor_currents[idJoint] = joints_[idJoint].getEffort(); DataOneIter_.motor_currents[idJoint] = joints_[idJoint].getEffort();
} }
...@@ -840,16 +889,17 @@ namespace sot_controller ...@@ -840,16 +889,17 @@ namespace sot_controller
/// Find the joint /// Find the joint
std::string joint_name = joints_name_[idJoint]; std::string joint_name = joints_name_[idJoint];
/// Find the related data /// Find the related data
std::map<std::string,EffortControlPDMotorControlData>::iterator std::map<std::string,double>::iterator
search_ecpd = effort_mode_pd_motors_.find(joint_name); search_ecpd = desired_init_pose_.find(joint_name);
if (search_ecpd!=effort_mode_pd_motors_.end()) if (search_ecpd!=desired_init_pose_.end())
{ {
EffortControlPDMotorControlData & ecpdcdata = double ecpdcdata = search_ecpd->second;
search_ecpd->second;
/// Control the joint accordingly /// Control the joint accordingly
joints_[idJoint].setCommand(ecpdcdata.des_pos); joints_[idJoint].setCommand(ecpdcdata);
} }
else
ROS_ERROR("Unable to find init pose for joint %s", joint_name);
} }
} }
......
...@@ -144,8 +144,11 @@ namespace sot_controller ...@@ -144,8 +144,11 @@ namespace sot_controller
/// \brief Implement a PD controller for the robot when the dynamic graph /// \brief Implement a PD controller for the robot when the dynamic graph
/// is not on. /// is not on.
std::map<std::string,EffortControlPDMotorControlData> effort_mode_pd_motors_; std::map<std::string, EffortControlPDMotorControlData> effort_mode_pd_motors_;
/// \brief Give the desired position when the dynamic graph is not on.
std::map<std::string, double> desired_init_pose_;
/// \brief Map from ros-control quantities to robot device /// \brief Map from ros-control quantities to robot device
/// ros-control quantities are for the sensors: /// ros-control quantities are for the sensors:
/// * motor-angles /// * motor-angles
...@@ -227,6 +230,9 @@ namespace sot_controller ...@@ -227,6 +230,9 @@ namespace sot_controller
/// \brief Read the PID information of the robot in effort mode. /// \brief Read the PID information of the robot in effort mode.
bool readParamsEffortControlPDMotorControlData(ros::NodeHandle &robot_nh); bool readParamsEffortControlPDMotorControlData(ros::NodeHandle &robot_nh);
/// \brief Read the desired initial pose of the robot in position mode.
bool readParamsPositionControlData(ros::NodeHandle &robot_nh);
/// \brief Read the control period. /// \brief Read the control period.
bool readParamsdt(ros::NodeHandle & robot_nh); bool readParamsdt(ros::NodeHandle & robot_nh);
///@} ///@}
......
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