Merge pull request #2 from dash-robotics/feature/dynamic_gains

Feature/dynamic gains

Author: ykarmesh

external/dynamixel-workbench-msgs/dynamixel_workbench_msgs/CMakeLists.txt

@@ -47,6 +47,7 @@ add_service_files(
   WheelCommand.srv
   GetDynamixelInfo.srv
   DynamixelCommand.srv
+  SetPID.srv
 )
 
 generate_messages(

external/dynamixel-workbench-msgs/dynamixel_workbench_msgs/srv/SetPID.srv

@@ -0,0 +1,8 @@
+# This message is used to set PID for different joints
+
+uint8 joint_num #Possible values: joint_1_id : 1, joint_2_1_id: 2, joint_2_2_id: 3, joint_3_id: 4 joint_4_id: 5, joint_5_id: 6 , gripper: 7, pan: 8, tilt: 9
+uint8 P
+uint8 I
+uint8 D
+---
+bool result

external/dynamixel-workbench/dynamixel_workbench_controllers/include/dynamixel_workbench_controllers/position_control.h

@@ -31,6 +31,7 @@
 #include <dynamixel_workbench_toolbox/dynamixel_workbench.h>
 #include <dynamixel_workbench_msgs/DynamixelStateList.h>
 #include <dynamixel_workbench_msgs/JointCommand.h>
+#include <dynamixel_workbench_msgs/SetPID.h>
 
 // Probably should make these constexpr
 #define GRIPPER_OPEN_MOTOR_POS -1.147412
@@ -66,6 +67,7 @@ class PositionControl
 
   // ROS Service Server
   ros::ServiceServer joint_command_server_;
+  ros::ServiceServer set_pid_server_;
 
   // ROS Service Client
 
@@ -128,6 +130,7 @@ class PositionControl
   void initServer();
   // bool jointCommandMsgCallback(dynamixel_workbench_msgs::JointCommand::Request &req,
                                // dynamixel_workbench_msgs::JointCommand::Response &res);
+  bool setPIDMsgCallback(dynamixel_workbench_msgs::SetPID::Request &req,dynamixel_workbench_msgs::SetPID::Response &res);
   void goalJointPositionCallback(const sensor_msgs::JointState::ConstPtr &msg);
   void gripperCloseCallback(const std_msgs::Empty::ConstPtr &msg);
   void gripperOpenCallback(const std_msgs::Empty::ConstPtr &msg);

external/dynamixel-workbench/dynamixel_workbench_controllers/src/position_control.cpp

@@ -280,6 +280,7 @@ void PositionControl::initSubscriber()
 void PositionControl::initServer()
 {
     // joint_command_server_ = node_handle_.advertiseService("joint_command", &PositionControl::jointCommandMsgCallback, this);
+    set_pid_server_ = node_handle_.advertiseService("SetPID", &PositionControl::setPIDMsgCallback, this);
 }
 
 void PositionControl::dynamixelStatePublish()
@@ -512,29 +513,15 @@ void PositionControl::controlLoop()
     cameraStatePublish();
 }
 
-// bool PositionControl::jointCommandMsgCallback(dynamixel_workbench_msgs::JointCommand::Request &req,
-//                                               dynamixel_workbench_msgs::JointCommand::Response &res)
-// {
-//   int32_t goal_position = 0;
-//   int32_t present_position = 0;
-
-//   if (req.unit == "rad")
-//   {
-//     goal_position = dxl_wb_->convertRadian2Value(req.id, req.goal_position);
-//   }
-//   else if (req.unit == "raw")
-//   {
-//     goal_position = req.goal_position;
-//   }
-//   else
-//   {
-//     goal_position = req.goal_position;
-//   }
-
-//   bool ret = dxl_wb_->goalPosition(req.id, goal_position);
+bool PositionControl::setPIDMsgCallback(dynamixel_workbench_msgs::SetPID::Request &req,dynamixel_workbench_msgs::SetPID::Response &res)
+{
+    int joint_num = req.joint_num;
+    dxl_wb_arm_->itemWrite(dxl_id_arm_[joint_num], "Position_P_Gain", req.P);
+    dxl_wb_arm_->itemWrite(dxl_id_arm_[joint_num], "Position_I_Gain", req.I);
+    dxl_wb_arm_->itemWrite(dxl_id_arm_[joint_num], "Position_D_Gain", req.D);
 
-//   res.result = ret;
-// }
+    res.result = true;
+}
 
 void PositionControl::goalJointPositionCallback(const sensor_msgs::JointState::ConstPtr& msg)
 {

scripts/ik_control.py

@@ -12,7 +12,7 @@
 from std_msgs.msg import Float64
 from std_msgs.msg import Empty
 from trac_ik_python.trac_ik import IK
-# import camera_forward_kinematics 
+# import camera_forward_kinematics
 import tf
 import PRM
 
@@ -29,6 +29,7 @@
 IK_POSITION_TOLERANCE = 0.01
 IK_ORIENTATION_TOLERANCE = np.pi/9
 current_joint_state = None
+MIN_CLOSING_GAP = 0.002
 
 
 def home_arm(pub):
@@ -56,8 +57,10 @@ def set_arm_joint(pub, joint_target):
 
 def get_joint_state(data):
     global current_joint_state
+    global current_gripper_state
     # if len(data.position) == 9:
-    current_joint_state = data.position[0:5]
+    current_gripper_state = data.positions[7:9]
+    current_joint_state = data.positions[0:5]
 
 def open_gripper(pub):
     empty_msg = Empty()
@@ -106,14 +109,14 @@ def compute_ik(ik_solver, target_pose, current_joint):
 def main():
     rospy.init_node('IK_Control', anonymous=True)
     global current_joint_state
-    
+
     rad_from_deg = np.pi/180.
     deg_from_rad = 180./np.pi
 
     ik_solver = IK(ARM_BASE_LINK, GRIPPER_LINK)
     # ck = camera_forward_kinematics.camerakinematics()
 
-    # Describing the publisher subscribers    
+    # Describing the publisher subscribers
     rospy.Subscriber('/joint_states', JointState, get_joint_state)
 
     arm_pub = rospy.Publisher(ROSTOPIC_SET_ARM_JOINT, JointState, queue_size=1)
@@ -129,7 +132,7 @@ def main():
     #home_joint = [0.0, 0.0, 1.22, -0.142, 0.0]
     set_arm_joint(arm_pub, home_joint)
     close_gripper(gripper_close_pub)
-    set_camera_angles(pan_pub, tilt_pub, rad_from_deg*0., rad_from_deg*40.0) #, ck)  
+    set_camera_angles(pan_pub, tilt_pub, rad_from_deg*0., rad_from_deg*40.0) #, ck)
 
     rospy.sleep(10)
 
@@ -143,11 +146,11 @@ def main():
             break
         except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
            print(e)
-    
+
     O = tf.transformations.euler_from_quaternion(Q)
     Q = tf.transformations.quaternion_from_euler(0, np.pi/2, O[2])
     Q1 = tf.transformations.quaternion_from_euler(0, np.pi/2, O[2]+np.pi/2)
-    
+
     poses = [Pose(Point(P[0], P[1], P[2]+0.20), Quaternion(Q[0], Q[1], Q[2], Q[3])),
              Pose(Point(P[0], P[1], P[2]+0.15), Quaternion(Q[0], Q[1], Q[2], Q[3])),
              Pose(Point(P[0], P[1], P[2]+0.25), Quaternion(Q[0], Q[1], Q[2], Q[3])),
@@ -187,9 +190,12 @@ def main():
             if itr == 0 or itr == 3:
                 open_gripper(gripper_open_pub)
                 rospy.sleep(4)
-            if itr == 1:                
+            if itr == 1:
                 close_gripper(gripper_close_pub)
                 rospy.sleep(4)
+                # Use the following condition to detect if the object is grasped or not
+                if(np.abs(current_gripper_state[0]) < MIN_CLOSING_GAP and np.abs(current_gripper_state[1] < MIN_CLOSING_GAP)):
+                    state = "Not grabbed"
         rospy.sleep(4)
         itr += 1
 
@@ -225,4 +231,4 @@ def main():
 
     # q = tf.transformations.quaternion_from_euler(0, 90, 0)
     # poses = [Pose(Point(0.3, 0.000, 0.25), Quaternion(q[0], q[1], q[2], q[3])),
-    #         Pose(Point(0.3, 0.000, 0.10), Quaternion(q[0], q[1], q[2], q[3]))]
+    #         Pose(Point(0.3, 0.000, 0.10), Quaternion(q[0], q[1], q[2], q[3]))]