Commit 273d484b authored by Alexandra Zobova's avatar Alexandra Zobova
Browse files

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

parent 2e4d28ef
......@@ -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
......
//---------------------------
// 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
......@@ -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 -- //
......
This diff is collapsed.
Supports Markdown
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