Illustration of 3D Contact. Arms' joints reference control for taking a box and raising it up'

StandaloneC/src/project/controller_files/arm_pos_ref.c

@@ -33,8 +33,13 @@
 #define MAX_ELB  (   0.0 * DEG_TO_RAD )
 #define DIFF_ELB (MAX_ELB - MIN_ELB)
 
-#if !defined(STANDALONE)
+#define SimpleManipulation
+
+#ifdef SimpleManipulation
+#include "manipulation_arm_pos.h"
+#endif
 
+#if !defined(STANDALONE)
 // time
 #define TIME_1 2.0
 #define TIME_2 10.0
@@ -43,6 +48,7 @@
 #define TIME_5 40.0
 #define TIME_6 50.0
 
+
 /*
  * Arms position references for the Simulink version
  */
@@ -170,8 +176,13 @@ void arm_pos_ref(ControllerStruct *cvs)
 #elif !defined(SDL)
 void arm_pos_ref(ControllerStruct *cvs)
 {
+
+    double tsim;
+
+	tsim = cvs->Inputs->t;
+
     // left arm position reference [rad]
-    cvs->q_ref_l_sh_sag =  MIN_SH_SAG + 0.5 * DIFF_SH_SAG;
+    cvs->q_ref_l_sh_sag = 0;// MIN_SH_SAG + 0.5 * DIFF_SH_SAG;
     cvs->q_ref_l_sh_lat = -MAX_SH_LAT + 0.5 * DIFF_SH_LAT;
     cvs->q_ref_l_sh_yaw = -MAX_SH_YAW + 0.5 * DIFF_SH_YAW;
     cvs->q_ref_l_elb    =  MIN_ELB    + 0.5 * DIFF_ELB;
@@ -182,6 +193,53 @@ void arm_pos_ref(ControllerStruct *cvs)
     cvs->q_ref_r_sh_yaw = MAX_SH_YAW - 0.5 * DIFF_SH_YAW;
     cvs->q_ref_r_elb    = MIN_ELB    + 0.5 * DIFF_ELB;
 
+    #ifdef SimpleManipulation
+
+    if (tsim < Time_Contact)
+    {
+    // right arm position reference [rad]
+    cvs->q_ref_r_sh_sag = linear_interpolation(tsim, 0, Time_Contact, 0.0, RShSag_Contact); //RShSag_Contact;
+    cvs->q_ref_r_sh_lat = linear_interpolation(tsim, 0, Time_Contact, 0.0, RShLat_Contact); //RShLat_Contact;
+    cvs->q_ref_r_sh_yaw = linear_interpolation(tsim, 0, Time_Contact, 0.0, RShYaw_Contact); //RShYaw_Contact;
+    cvs->q_ref_r_elb    = linear_interpolation(tsim, 0, Time_Contact, 0.0, RElb_Contact); //RElb_Contact;
+
+    // left arm position reference [rad]
+    // the left arm goes symmetrically RShSag = LShSag; RShLat = - LShLat; RShYaw = - LShYaw; RElb = LElb
+ 
+    cvs->q_ref_l_sh_sag = linear_interpolation(tsim, 0, Time_Contact, 0.0, RShSag_Contact); //RShSag_Contact;
+    cvs->q_ref_l_sh_lat = linear_interpolation(tsim, 0, Time_Contact, 0.0, -RShLat_Contact); //RShLat_Contact;
+    cvs->q_ref_l_sh_yaw = linear_interpolation(tsim, 0, Time_Contact, 0.0, -RShYaw_Contact); //RShYaw_Contact;
+    cvs->q_ref_l_elb    = linear_interpolation(tsim, 0, Time_Contact, 0.0, RElb_Contact); //RElb_Contact;
+
+    /*cvs->q_ref_l_sh_sag =  RShSag_Contact;
+    cvs->q_ref_l_sh_lat = -RShLat_Contact;
+    cvs->q_ref_l_sh_yaw = -RShYaw_Contact;
+    cvs->q_ref_l_elb    =  RElb_Contact;*/
+    }
+    else
+    {
+
+    cvs->q_ref_r_sh_sag = linear_interpolation(tsim,  Time_Contact, Time_Up, RShSag_Contact,RShSag_Up); //RShSag_Contact;
+    cvs->q_ref_l_sh_sag = linear_interpolation(tsim,  Time_Contact, Time_Up, RShSag_Contact,RShSag_Up); //RShSag_Contact;
+
+/*     cvs->q_ref_r_sh_sag = RShSag_Up;
+     cvs->q_ref_l_sh_sag =  RShSag_Up;
+*/
+
+//right arm
+    cvs->q_ref_r_sh_lat =  RShLat_Contact;
+    cvs->q_ref_r_sh_yaw =  RShYaw_Contact;
+    cvs->q_ref_r_elb    =  RElb_Contact;
+//left arm    
+    cvs->q_ref_l_sh_lat = -RShLat_Contact;
+    cvs->q_ref_l_sh_yaw = -RShYaw_Contact;
+    cvs->q_ref_l_elb    =  RElb_Contact;
+ 
+
+    }
+
+    #endif
+
     // correction for the long arms case
     #ifdef LONG_ARMS
 

StandaloneC/src/project/controller_files/manipulation_arm_pos.h

+//---------------------------
+// Creation : 04/06/2015
+// author: Alexandra Zobova
+//To illustrate 3D Simbody contact algorithm on a simple manipulation task
+//
+// defines the right arm's joint positions to take a box from a table and to raise it up
+// the left arm goes symmetrically RShSag = LShSag; RShLat = - LShLat; RShYaw = - LShYaw; RElb = LElb
+//---------------------------
+
+// joints' value to touch a box 
+#define RShSag_Contact -0.172
+#define RShLat_Contact -0.25
+#define RShYaw_Contact 0.74
+#define RElb_Contact -1.328
+
+#define Time_Contact 0.3
+#define Time_Up 1
+
+#define RShSag_Up -0.7
+
+

StandaloneC/src/project/project_files/info_project.h

@@ -28,7 +28,7 @@
 
 // simulation time
 #define TSIM_INIT  0.0    // inital time of the simulation  [s]
-#define TSIM_END   60.0   // end time of the simulation     [s]
+#define TSIM_END   1.5   // end time of the simulation     [s]
 
 
 // -- Fixed time step integrator -- //