Drive a Segment (with SpericalJoint to Ground) by position

Main Forum
1

Hello again,

I'm trying to understand how to drive a segment (connected with spherical joints) based on its initial and target XYZ positions. Is this generally possible?

I've started with a simple example: a single segment connected to the ground using a 3DOF joint. However, I haven't been able to get it working. It does work when I use revolute joints instead.

Could you assist me with this? I understand I could work with rotational inputs, but as part of a larger project, I'm aiming to implement drivers that move segments directly to target positions rather than controlling them through rotations.

Here is the code I'm working with:

Main = {

AnyFolder Model = {
// Global Ref
AnyFixedRefFrame GlobalRef = {
AnyDrawRefFrame drw = {
ScaleXYZ = {0.05, 0.05, 0.05};
RGB = {1,0,0};
};
};

// Segment1
AnySeg Seg1 = {
  r0 = {0,0.1,0.1};
  Mass = 1;
  Jii = {0.0005, 0.0005, 0.0005};
  viewNodes.Visible = On;
  viewNodes.Opacity = 0.5;
  
  AnyRefNode CenterNode = {
    viewRefFrame.Visible = On;
    viewRefFrame.RGB = {0,1,0};
  };
  
  AnyRefNode JntNode = {
    sRel = {0, -0.1, -0.1};
    viewRefFrame.Visible = On;
  };
}; //Segment


// Joints
AnySphericalJoint Jnt1 = {
  AnyFixedRefFrame& ref1 = Main.Model.GlobalRef;
  AnyRefFrame& ref2 = Main.Model.Seg1.JntNode;
  viewJoint.Visible = On;
}; //Joints

//Length Jnt1 
AnyKinLinear lengthjnt1 = {
  Ref = 0;
  AnyFixedRefFrame &ref1 = Main.Model.GlobalRef;
  AnyRefFrame &ref2 = Main.Model.Seg1.CenterNode;
}; //Measures

// Measures
AnyKinLinear segpos1 = {
  Ref = 0;
  AnyFixedRefFrame &ref1 = Main.Model.GlobalRef;
  AnyRefFrame &ref2 = Main.Model.Seg1.CenterNode;
}; //Measures

// Driver for Sperhical Joint
AnyKinEqInterPolDriver driver1xyz = {
  AnyKinLinear &kin = .segpos1;
  Type = PiecewiseLinear;
  T = {0,1};
  Data = {
    {0.0, 0.0},  //x: x0=0 and x1=0.1
    {0.1, -0.1},  //y: y0=0.1 and y1=0.0
    {0.1, -0.1}  //z: z0=0 and z1=0.0
  };
  //CType = {Soft,Soft,Soft};
};//Driver

}; //Model

AnyBodyStudy Study = {
AnyFolder &model = Main.Model;
Gravity = {0, 0, 0};
};//Study
}; //Main

2

Hi Viktoria,

This is an interesting problem. It is possible but it depends a lot on how you interpolate between the initial and target positions. One of the problems that I faced in your code is that the given points for the interpolation driver create a very difficult situation. You have provided only two points that are diagonally opposite. In this case, as the solver iterates it ends up in a situation where the error increases as it tries to rotate the end point around the spherical joint. So, the initial position ends up remaining as the solution. Also, the interpolation drivers can be a little bit finicky at the boundary conditions, so you would want to have the study time slightly within the time period of the interpolation driver.

Apart from that, generally, I think another approach can be better. You can make a dummy segment that strictly follows the interpolation driver in the linear coordinates, while its orientation can be fixed to a constant value. Then, if you orient the joint node on the main segment such that one of the axes points to the end point, then you can make a driver to constraint the main segment to always point towards the dummy segment, which follows the interpolation driver.

Finally, another problem is that when you drive a spherical joint like this, you still are left with one degree of freedom that is unconstrained. If you imagine your segment to be a stick pointing to a point that is being driven by the interpolation driver, the rotation about the axis of the stick remains unconstrained. If this is relevant, then you must find another way to constrain it, otherwise, it can be driven to a constant value.

Here is the updated code with the changes described above:

Main = {
  
  AnyFolder Model = {
    // Global Ref
    AnyFixedRefFrame GlobalRef = {
      AnyDrawRefFrame drw = {
        ScaleXYZ = {0.05, 0.05, 0.05};
        RGB = {1,0,0};
      };
    };
    
    // Segment1
    AnySeg Seg1 = {
      r0 = {0,0.1,0.1};
      Mass = 1;
      Jii = {0.0005, 0.0005, 0.0005};
      viewNodes.Visible = On;
      viewNodes.Opacity = 0.5;
      
      AnyRefNode CenterNode = {
        viewRefFrame.Visible = On;
        viewRefFrame.RGB = {0,1,0};
      };
      
      AnyRefNode JntNode = {
        sRel = {0, -0.1, -0.1};
        // Orient JntNode to have one of the axes point to the other end
        ARel = RotMat(sRel,.CenterNode.sRel,{0,0.1,0.0});
        viewRefFrame.Visible = On;
      };
    }; //Segment
    
    AnySeg DummySeg = {
      Mass = 0.0;
      Jii = {0.0,0.0,0.0};
      r0 = {.driver1xyz.Xpos[0],.driver1xyz.Ypos[0],.driver1xyz.Zpos[0],};
      AnyDrawSeg drw = {};
    };
    
    // Joints
    AnySphericalJoint Jnt1 = {
      AnyFixedRefFrame& ref1 = Main.Model.GlobalRef;
      AnyRefFrame& ref2 = Main.Model.Seg1.JntNode;
      viewJoint.Visible = On;
    }; //Joints
    
//    //Length Jnt1 
//    AnyKinLinear lengthjnt1 = {
//      Ref = 0;
//      AnyFixedRefFrame &ref1 = Main.Model.GlobalRef;
//      AnyRefFrame &ref2 = Main.Model.Seg1.CenterNode;
//    }; //Measures
    
    // Measures
    AnyKinLinear segpos1 = {
      Ref = 0;
      AnyFixedRefFrame &ref1 = Main.Model.GlobalRef;
      AnyRefFrame &ref2 = Main.Model.Seg1.CenterNode;
    }; //Measures
    
    AnyKinLinear DummySegLin = {
      AnyRefFrame &ref = .DummySeg;
    };
    AnyKinRotational DummySegRot = {
      AnyRefFrame &ref = .DummySeg;
      Type = RotAxesAngles;
    };
    // This fixes the rotation of the dummy seg
    AnyKinEq FixDummySegRot = {
      AnyKinMeasure &rot = .DummySegRot;
    };
    // Measures between Segment and Dummy Segment
    AnyKinMeasureOrg SegToDummySeg = {
      AnyKinLinear Lin = {
        AnyRefFrame &Seg = ..Seg1.JntNode;
        AnyRefFrame &DummySeg = ..DummySeg;
        Ref = 0;
      };
      AnyKinRotational Rot = {
        AnyRefFrame &Seg = ..Seg1.JntNode;
        AnyRefFrame &DummySeg = ..DummySeg;
        Type = RotAxesAngles;
      };    
      MeasureOrganizer = {1,2,5}; // Consider 3: Lin Y, Z, and Rot X
    };
    // Driver for Sperhical Joint
    AnyKinEqInterPolDriver driver1xyz = {
      // Interpolation drivers can be difficult to solve at the boundary..
      // Therefore, the time for the driver is extended beyond the study time
      AnyFloat StartTime = Main.Study.tStart - 0.10;
      AnyFloat EndTime = Main.Study.tEnd + 0.1;
      // Start and End pos for the driver.
      AnyFloat Xpos = {0.0,0.07};
      AnyFloat Ypos = {0.1, -0.08};
      AnyFloat Zpos = {0.1,0.05};
      
      AnyInt Steps = 100;
      
      AnyFloat time = linspace(StartTime,EndTime,Steps);
      AnyFloat XVal = linspace(Xpos[0],Xpos[1],Steps);
      AnyFloat YVal = linspace(Ypos[0],Ypos[1],Steps);
      AnyFloat ZVal = linspace(Zpos[0],Zpos[1],Steps);
      
      //      AnyKinLinear &kin = .segpos1;
      AnyKinLinear &kin = .DummySegLin;
      Type = PiecewiseLinear;
      //      T = {0,1};
      //      Data = {
      //        {0.0, 0.1},  //x: x0=0 and x1=0.1
      //        {0.1, 0.0},  //y: y0=0.1 and y1=0.0
      //        {0.1, -0.1}  //z: z0=0 and z1=0.0
      //      };
      
      T = time;
      Data = {XVal,YVal,ZVal};
      //      CType = {Soft,Soft,Soft}; // This is not needed if driven using dummy seg
    };//Driver
    
    // This driver fixes the measure between seg and dummy seg. They
    // are in Lin Y, Lin Z and Rot X. Rot X can be driven in another way.
    AnyKinEq SphericalJointDriver = {
      AnyKinMeasureOrg &Meas =.SegToDummySeg;
    };
  }; //Model
  
  AnyBodyStudy Study = {
    AnyFolder &model = Main.Model;
    Gravity = {0, 0, 0};
  };//Study
}; //Main

Best regards,
Dave