Skip to content

GPU Forward Kinematics and Collision checkin #4

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
cblappert wants to merge 3 commits into
base: master
Choose a base branch
from
Open

GPU Forward Kinematics and Collision checkin #4

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
2280098
GPU Forward Kinematics done.
May 30, 2016
949f9b2
Created new accessor methods
Jun 1, 2016
c16e151
Added documentation, added intersection function
Jun 9, 2016
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -87,6 +87,7 @@ set(dart_src

${PROJECT_SOURCE_DIR}/src/optimization/kernels/obsToMod.h
${PROJECT_SOURCE_DIR}/src/optimization/kernels/modToObs.h
${PROJECT_SOURCE_DIR}/src/optimization/kernels/kinematics.h
${PROJECT_SOURCE_DIR}/src/optimization/kernels/intersection.h
${PROJECT_SOURCE_DIR}/src/optimization/kernels/raycast.h
${PROJECT_SOURCE_DIR}/src/optimization/contact_prior.cpp
Expand Down Expand Up @@ -152,6 +153,7 @@ set(gpu_specific_src
${PROJECT_SOURCE_DIR}/src/optimization/kernels/kernel_common.h
${PROJECT_SOURCE_DIR}/src/optimization/kernels/obsToMod.cu
${PROJECT_SOURCE_DIR}/src/optimization/kernels/modToObs.cu
${PROJECT_SOURCE_DIR}/src/optimization/kernels/kinematics.cu
${PROJECT_SOURCE_DIR}/src/optimization/kernels/intersection.cu
${PROJECT_SOURCE_DIR}/src/optimization/kernels/raycast.cu

Expand Down
7 changes: 7 additions & 0 deletions src/model/mirrored_model.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -86,6 +86,13 @@ class MirroredModel : public Model {

inline const SE3 getTransformJointAxisToParent(const int joint) const { return _T_pf->hostPtr()[joint]; }

// Accessors for the kinematics code
inline const SE3 *getTransformsParentJointToFrame() const { return _T_pf->hostPtr(); }
inline const SE3 *getDeviceTransformsParentJointToFrame() const { return _T_pf->devicePtr(); }
inline const void syncKinematics() { _T_mf->syncDeviceToHost(); }
inline const void syncKinematicsHostToDevice() { _T_mf->syncHostToDevice(); }
inline SE3 *getTransformsFrameToModel() { return _T_mf->hostPtr(); }

private:

uint _modelID;
Expand Down
1 change: 1 addition & 0 deletions src/model/model.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -99,6 +99,7 @@ class Model {

inline float getJointMin(const int joint) const { return _jointLimits[joint].x; }
inline float getJointMax(const int joint) const { return _jointLimits[joint].y; }
inline const std::vector<float2> &getJointLimits() const { return _jointLimits; }

const std::string & getJointName(const int joint) const { return _jointNames[joint]; }
void renderSdf(const dart::Grid3D<float> & sdf, float levelSet) const;
Expand Down
177 changes: 176 additions & 1 deletion src/optimization/kernels/intersection.cu
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -144,6 +144,84 @@ __global__ void gpu_normEqnsSelfIntersection(const float4 * testSites,

}

// Computes a single intersection
template <bool dbgErr>
__global__ void gpu_normEquationsIntersectionRaw(const float4 * testSites,
const int nSites,
const SE3 T_ds,
const SE3 T_sd,
const SE3 * T_mfs_src,
const SE3 * T_fms_src,
const int nFramesSrc,
const int * sdfFrames_src,
const SE3 * T_mfs_dst,
const SE3 * T_fms_dst,
const int nFramesDst,
const int nConfigs,
const int * sdfFrames_dst,
const Grid3D<float> * sdfs_dst,
const int nSdfs_dst,
float * result,
int * resultSdf,
bool vary_dst,
float * debugError) {

const int index = blockIdx.x*blockDim.x + threadIdx.x;
const int configIdx = blockIdx.y*blockDim.y + threadIdx.y;
// index corresponds to which pair of T_mfs_src, T_mfs_dst we are using
if (configIdx >= nConfigs) {
return;
}
// setup pointers correctly
T_mfs_src = T_mfs_src + configIdx * nFramesSrc;
T_fms_src = T_fms_src + configIdx * nFramesSrc;
if (vary_dst) {
T_mfs_dst = T_mfs_dst + configIdx * nFramesDst;
T_fms_dst = T_fms_dst + configIdx * nFramesDst;
}
result = result + configIdx * (nSites * nConfigs);
resultSdf = resultSdf + configIdx * (nSites * nConfigs);


// overflow
if (index >= nSites) {
return;
}

float4 v_src_f = testSites[index];
const int srcGrid = round(v_src_f.w);
const int srcFrame = sdfFrames_src[srcGrid];

v_src_f.w = 1;
const float4 v_src_m = T_mfs_src[srcFrame]*v_src_f;
const float4 v_dst_m = T_ds*v_src_m;

int dstIdx = -1;
float smallestResidual = 1.0e50;

for (int dstGrid=0; dstGrid<nSdfs_dst; ++dstGrid) {

const int dstFrame = sdfFrames_dst[dstGrid];
const float4 v_dst_f = T_fms_dst[dstFrame]*v_dst_m;

const Grid3D<float> & dstSdf = sdfs_dst[dstGrid];
const float3 v_dst_g = dstSdf.getGridCoords(make_float3(v_dst_f));

if (dstSdf.isInBoundsGradientInterp(v_dst_g)) {

const float residual = dstSdf.getValueInterpolated(v_dst_g)*dstSdf.resolution;
if (residual < smallestResidual) {
dstIdx = dstGrid;
smallestResidual = residual;
}
}

}
result[index] = smallestResidual;
resultSdf[index] = dstIdx;

}

__global__ void gpu_normEqnsSelfIntersectionReduced(const float4 * testSites,
const int nSites,
const int fullDims,
Expand Down Expand Up @@ -839,6 +917,104 @@ void normEqnsIntersection(const float4 * testSites,

}

void normEqnsIntersectionRaw(const float4 * testSites,
const int nSites,
const SE3 T_ds,
const SE3 T_sd,
const SE3 *src_T_mfs,
const SE3 *src_T_fms,
const int nFramesSrc, // number of frames per joint config
const SE3 *dst_T_mfs,
const SE3 *dst_T_fms,
const int nFramesDst, // number of frames per joint config
const int nConfigs,// number of total joint configs
const MirroredModel & srcModel,
const MirroredModel & dstModel,
float * result,
int * resultsdf,
float * debugError) {
cudaMemset(result,0,(nSites*nConfigs)*sizeof(float));
cudaMemset(resultsdf,1,(nSites*nConfigs)*sizeof(int));
dim3 block(64,64,1);
dim3 grid(ceil(nSites/(float)block.x),ceil(nConfigs/(float)block.y),1);
if (debugError == 0) {
gpu_normEquationsIntersectionRaw<false><<<grid,block>>>(testSites, nSites,
T_ds, T_sd,
src_T_mfs, src_T_fms,
nFramesSrc,
srcModel.getDeviceSdfFrames(),
dst_T_mfs, dst_T_fms,
nFramesDst,
nConfigs,
dstModel.getDeviceSdfFrames(),
dstModel.getDeviceSdfs(),
dstModel.getNumSdfs(),
result, resultsdf, true, debugError);
} else {
gpu_normEquationsIntersectionRaw<true><<<grid,block>>>(testSites, nSites,
T_ds, T_sd,
src_T_mfs, src_T_fms,
nFramesSrc,
srcModel.getDeviceSdfFrames(),
dst_T_mfs, dst_T_fms,
nFramesDst,
nConfigs,
dstModel.getDeviceSdfFrames(),
dstModel.getDeviceSdfs(),
dstModel.getNumSdfs(),
result, resultsdf, true, debugError);
}
}

void normEqnsIntersectionRawSingleTgt(const float4 * testSites,
const int nSites,
const SE3 T_ds,
const SE3 T_sd,
const SE3 *src_T_mfs,
const SE3 *src_T_fms,
const int nFramesSrc, // number of frames per joint config
const SE3 *dst_T_mf,
const SE3 *dst_T_fm,
const int nFramesDst, // number of frames per joint config
const int nConfigs,// number of total joint configs
const MirroredModel & srcModel,
const MirroredModel & dstModel,
float * result,
int * resultsdf,
float * debugError) {
cudaMemset(result,0,(nSites*nConfigs)*sizeof(float));
cudaMemset(resultsdf,1,(nSites*nConfigs)*sizeof(int));
dim3 block(64,64,1);
dim3 grid(ceil(nSites/(float)block.x),ceil(nConfigs/(float)block.y),1);
if (debugError == 0) {
gpu_normEquationsIntersectionRaw<false><<<grid,block>>>(testSites, nSites,
T_ds, T_sd,
src_T_mfs, src_T_fms,
nFramesSrc,
srcModel.getDeviceSdfFrames(),
dst_T_mf, dst_T_fm,
nFramesDst,
nConfigs,
dstModel.getDeviceSdfFrames(),
dstModel.getDeviceSdfs(),
dstModel.getNumSdfs(),
result, resultsdf, false, debugError);
} else {
gpu_normEquationsIntersectionRaw<true><<<grid,block>>>(testSites, nSites,
T_ds, T_sd,
src_T_mfs, src_T_fms,
nFramesSrc,
srcModel.getDeviceSdfFrames(),
dst_T_mf, dst_T_fm,
nFramesDst,
nConfigs,
dstModel.getDeviceSdfFrames(),
dstModel.getDeviceSdfs(),
dstModel.getNumSdfs(),
result, resultsdf, false, debugError);
}
}


void normEqnsSelfIntersectionReduced(const float4 * testSites,
const int nSites,
Expand Down Expand Up @@ -994,5 +1170,4 @@ void initDebugIntersectionError(float * debugError,
gpu_initDebugIntersectionError<<<grid,block>>>(debugError, nSites);

}

}
56 changes: 56 additions & 0 deletions src/optimization/kernels/intersection.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,62 @@ void normEqnsIntersection(const float4 * testSites,
float * result,
float * debugError = 0);

/*
* src_T_mfs, src_T_fms, dst_T_mfs, dst_T_fms are all arrays of transforms on the
* gpu as computed by computeKinematicsBatchGPU, they are both nConfigs arrays
* of model-frame and frame-model transforms of their respective models
*
* result and resultsdf are nconfigs * nsites long arrays on the gpu to be used
* for results of distance checking. (result + config_idx * nsites)[site_idx] is
* the way to access a particular result for joint config config_idx and testSite
* site_idx. (Note that the memory should be copied over to the cpu first if that
* is where you intend to read the memory.)
* */
void normEqnsIntersectionRaw(const float4 * testSites,
const int nSites,
const SE3 T_ds,
const SE3 T_sd,
const SE3 *src_T_mfs,
const SE3 *src_T_fms,
const int nFramesSrc, // number of frames per joint config
const SE3 *dst_T_mfs,
const SE3 *dst_T_fms,
const int nFramesDst, // number of frames per joint config
const int nConfigs,// number of total joint configs
const MirroredModel & srcModel,
const MirroredModel & dstModel,
// distance to closest object
// results array is a block of results (per joint config) with layout paralleling the joint configs
// (nsites results for jtconfig 1 at result, nsites results for jtconfig 2 at result + nsites)
float * result, // should be an nsites * nconfigs long array that has already been allocated on the gpu
// index of the sdf corresponding to above
int * resultsdf, // should be an nsites * nconfigs long array that has already been allocated on the gpu
float * debugError = 0);

// same as above except your destination to intersect with is a single object instead of a
// multitude of joint configurations
void normEqnsIntersectionRawSingleTgt(const float4 * testSites,
const int nSites,
const SE3 T_ds,
const SE3 T_sd,
const SE3 *src_T_mfs,
const SE3 *src_T_fms,
const int nFramesSrc, // number of frames per joint config
const SE3 *dst_T_mf,
const SE3 *dst_T_fm,
const int nFramesDst, // number of frames per joint config
const int nConfigs,// number of total joint configs
const MirroredModel & srcModel,
const MirroredModel & dstModel,
// distance to closest object
// results array is a block of results (per joint config) with layout paralleling the joint configs
// (nsites results for jtconfig 1 at result, nsites results for jtconfig 2 at result + nsites)
float * result, // should be an nsites * nconfigs long array that has already been allocated on the gpu
// index of the sdf corresponding to above
int * resultsdf, // should be an nsites * nconfigs long array that has already been allocated on the gpu
float * debugError = 0);


void normEqnsIntersectionReduced(const float4 * testSites,
const int nSites,
const int fullDims,
Expand Down
Loading