Discussion: Add an orientation to robot trunk

[Yiming-Zhao]

When the robot walks on the terrain with high slope, then the motion looks unrealistic and the limbs start inverse the direction. I consider to add a extra orientation which will orientate the robot trunk parallel to the terrain surface. It would make the motion look better.

Videos

![with out orrientation]() ![with orrientation]()

[eastskykang]

Brilliant! Can I use your video for the next time?

[Yiming-Zhao]

Sure, feel free to use it and the link will remain open

[Hustwireless]

Hi Yiming, I've also tried to implement the base orientation as well, however the result is that the orientation looks like it's descritized, and something must be wrong. Could I may be have a look at your code snippet of this part? Thanks!

[Yiming-Zhao]

@Hustwireless in SimpleLocomotionTrajectoryPlanner.h

    Quaternion getOrientationfromTerrainNormalatTrunkPosition(double t)
    {   
        double ds=0.2;
        P3D pos=getTargetTrunkPositionAtTime(t);
        double HeadingAngle=getTargetTrunkHeadingAtTime(t);
        V3D forward= rotateVec(robot->forward,HeadingAngle,V3D(0, 1, 0));
        V3D sideways= rotateVec(RBGlobals::worldUp.cross(robot->forward),HeadingAngle,V3D(0, 1, 0));
        double dx=TerrainMeshHeight(pos+ds*forward)-TerrainMeshHeight(pos-ds*forward);
        double dy=TerrainMeshHeight(pos+ds*sideways)-TerrainMeshHeight(pos-ds*sideways);
        double angle_x=std::atan (-dx/(2*ds));
        double angle_y=std::atan (dy/(2*ds));

        return getRotationQuaternion(angle_y,forward)*getRotationQuaternion(angle_x,sideways);
    }
    virtual Quaternion getTargetTrunkOrientationAtTime(double t) {
     return getOrientationfromTerrainNormalatTrunkPosition(t)*getRotationQuaternion(getTargetTrunkHeadingAtTime(t),                                 V3D(0, 1, 0)) ;}

Notice: the order of quaternion is important