GIAdog-Bullet/RoboSim.cpp at master · eduardo98m/GIAdog-Bullet · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
#include "RoboSim.hpp"
#include <stdio.h>
#include "SharedMemory/PhysicsClientC_API.h"
#include "SharedMemory/b3RobotSimulatorClientAPI_InternalData.h"
#include <vector>
//#include "SharedMemory/SharedMemoryPublic.h"

RoboSim::RoboSim()
{
}

RoboSim::~RoboSim()
{
}

int RoboSim::loadURDF(const std::string &fileName,
                      const btVector3 &pos,
                      const btQuaternion &orn,
                      bool useMultiBody,
                      bool useFixedBase,
                      double globalScaling,
                      int flags){

    b3SharedMemoryStatusHandle statusHandle;

    b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;

    int statusType;

    flags |= URDF_USE_INERTIA_FROM_FILE;
    flags |= URDF_ENABLE_CACHED_GRAPHICS_SHAPES;
    flags |= URDF_IGNORE_VISUAL_SHAPES;
    b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(sm, fileName.c_str());

    b3LoadUrdfCommandSetFlags(command, flags);

    // setting the initial position, orientation and other arguments are
    // optional
    b3LoadUrdfCommandSetStartPosition(command, pos.getX(), pos.getY(), pos.getZ());

    b3LoadUrdfCommandSetStartOrientation(command, orn.getX(), orn.getY(), orn.getZ(), orn.getW());

    if (useMultiBody)
    {
        b3LoadUrdfCommandSetUseMultiBody(command, 1);
    }
    if (useFixedBase)
    {
        b3LoadUrdfCommandSetUseFixedBase(command, 1);
    }
    if (globalScaling > 0)
    {
        b3LoadUrdfCommandSetGlobalScaling(command, globalScaling);
    }
    statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
    statusType = b3GetStatusType(statusHandle);
    if (statusType != CMD_URDF_LOADING_COMPLETED)
    {
        printf("Cannot load URDF file.");
        return NULL;
    }
    int bodyUniqueId = b3GetStatusBodyIndex(statusHandle);

    return bodyUniqueId;
}


void RoboSim::setGravity(const btVector3 &gravityAcceleration){

    b3SharedMemoryStatusHandle statusHandle;

    b3SharedMemoryCommandHandle command;

    b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;

    command = b3InitPhysicsParamCommand(sm);

    b3PhysicsParamSetGravity(command, gravityAcceleration.getX(), gravityAcceleration.getY(), gravityAcceleration.getZ());

    statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
}

 int RoboSim::loadHeightfield(std::vector<float> height_map,
                              int numHeightfieldRows,
                              int numHeightfieldColumns,
                              double meshScaleX,
                              double meshScaleY,
                              double meshScaleZ,
                              double heightfieldTextureScaling,
                              int replaceHeightfieldIndex,
                              const btVector3 &pos,
                              const btQuaternion &orn){

    b3SharedMemoryStatusHandle statusHandle;

    b3SharedMemoryCommandHandle command;

    b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;

    command = b3CreateCollisionShapeCommandInit(sm);


    // Check if the size of the heightfield data matches the number of rows and columns
    if (height_map.size() != numHeightfieldColumns*numHeightfieldRows)
    {
        printf("Size of heightfieldData doesn't match numHeightfieldColumns*numHeightfieldRows");
        return NULL;
    }


    double meshScale[3] = {meshScaleX, meshScaleY, meshScaleZ};
    int shapeIndex = b3CreateCollisionShapeAddHeightfield2(sm,
                                                           command,
                                                           meshScale,
                                                           heightfieldTextureScaling,
                                                           height_map.data(),
                                                           numHeightfieldRows,
                                                           numHeightfieldColumns,
                                                           replaceHeightfieldIndex);

    double  collisionFramePosition    [3] = {pos.getX(), pos.getY(), pos.getZ()};
    double  collisionFrameOrientation [4] = {orn.getX(), orn.getY(), orn.getZ(), orn.getW()};
    double baseInertialFramePosition[3] = {0, 0, 0};
	double baseInertialFrameOrientation[4] = {0, 0, 0, 1};


    // b3CreateCollisionShapeSetChildTransform(command,
    //                                         shapeIndex,
    //                                         collisionFramePosition,
    //                                         collisionFrameOrientation);


    // int flags = 0;
    // b3CreateCollisionSetFlag(command, shapeIndex, flags);

    statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);

    int statusType = b3GetStatusType(statusHandle);
    printf("statusType = %d \n", statusType);
    if (statusType != CMD_CREATE_COLLISION_SHAPE_COMPLETED)
    {
        printf("Cannot create heightfield.");
        return NULL;
    }
    // Part B: Create a multi-body base using the heightfield shape

    printf("shapeIndex = %d \n", shapeIndex);
    shapeIndex = b3GetStatusCollisionShapeUniqueId(statusHandle);
    printf("shapeIndex = %d\n", shapeIndex);

    b3SharedMemoryCommandHandle command_b = b3CreateMultiBodyCommandInit(sm);
    printf("Debug A\n");
    int baseIndex = b3CreateMultiBodyBase(command_b,
                                            0.0,
                                            shapeIndex,
                                            -1,
                                            collisionFramePosition,
                                            collisionFrameOrientation,
                                            baseInertialFramePosition,
                                            baseInertialFrameOrientation);
    printf("Debug B\n");
    statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command_b);
    statusType = b3GetStatusType(statusHandle);
    printf("Debug c status handle:%d\n", statusType);


    return b3GetStatusCollisionShapeUniqueId(statusHandle);

 }