This is a follow-up of another similar issue. Only in my case, I am seeing >2 geometry ids not only for a link, but also its collision element.
In the code below, I am printing out geometry id's that are valid collision pairs:
#include "drake/multibody/inverse_kinematics/inverse_kinematics.h"
#include "drake/math/rotation_matrix.h"
#include "drake/solvers/create_constraint.h"
#include "drake/solvers/solve.h"
#include "drake/multibody/parsing/parser.h"
// #include "drake/solvers/clp_solver.h"
// #include "drake/solvers/csdp_solver.h"
#include "drake/solvers/gurobi_solver.h"
#include "drake/solvers/ipopt_solver.h"
#include "drake/solvers/mosek_solver.h"
#include "drake/solvers/snopt_solver.h"
#include "drake/solvers/solver_options.h"
#include "drake/solvers/nlopt_solver.h"
#include "drake/geometry/proximity_properties.h"
#include "drake/geometry/scene_graph_inspector.h"
#include "drake/geometry/shape_specification.h"
// #include "drake/multibody/tree/multibody_tree_indexes.h"
#include "drake/systems/analysis/simulator.h"
#include "drake/visualization/visualization_config_functions.h"
#include <iostream>
#include <boost/filesystem.hpp>
#include <fstream>
#include <chrono>
#include <numeric>
#include <bits/stdc++.h>
// using namespace boost::filesystem;
using namespace std;
int main() {
string sdf_filepath = "path_to_sdf.sdf";
drake::systems::DiagramBuilder<double> builder;
drake::multibody::MultibodyPlant<double>* plant{};
drake::geometry::SceneGraph<double>* scene_graph{};
std::tie(plant, scene_graph) = drake::multibody::AddMultibodyPlantSceneGraph(&builder, timestep);
plant->set_name("plant");
scene_graph->set_name("scene_graph");
drake::multibody::Parser parser(plant, scene_graph);
const auto robot_model_index = parser.AddModelFromFile(sdf_filepath, "robot");
plant->Finalize();
auto diagram = builder.Build();
auto diagram_context= diagram->CreateDefaultContext();
auto plant_context = &(diagram->GetMutableSubsystemContext(*plant,
diagram_context.get()));
const auto& inspector = scene_graph->model_inspector();
auto collision_pairs = inspector.GetCollisionCandidates();
for (auto& pair : collision_pairs) {
cout << pair.first << ", " << pair.second << endl;
const auto first_geom_id = pair.first;
const auto second_geom_id = pair.first;
const auto first_name = inspector.GetName(first_geom_id);
const auto second_name = inspector.GetName(second_geom_id);
cout << first_name << ", " << second_name << endl;
cout << "Left: " << endl;
const auto first_frame_id = inspector.GetFrameId(first_geom_id);
const auto first_geom_model_index = inspector.GetFrameGroup(first_frame_id);
const auto first_source_name = inspector.GetOwningSourceName(first_frame_id);
// NOTE: CUSTOM MODIFIED FUNCTION, essentially making GeometryState::get_solver_id() public
const auto first_source_id = inspector.GetOwningSourceId(first_geom_id);
cout << "frame_id: " << first_frame_id << endl;
cout << "geom_model_index: " << first_geom_model_index << endl;
cout << "robot_model_index: " << robot_model_index << endl;
cout << "source_name: " << first_source_name << endl;
cout << "source_id: " << first_source_id << endl;
cout << endl;
cout << "Right: " << endl;
const auto second_frame_id = inspector.GetFrameId(second_geom_id);
const auto second_geom_model_index = inspector.GetFrameGroup(second_frame_id);
const auto second_source_name = inspector.GetOwningSourceName(second_frame_id);
const auto second_source_id = inspector.GetOwningSourceId(second_geom_id);
cout << "frame_id: " << second_frame_id << endl;
cout << "geom_model_index: " << second_geom_model_index << endl;
cout << "robot_model_index: " << robot_model_index << endl;
cout << "source_name: " << second_source_name << endl;
cout << "source_id: " << second_source_id << endl;
cout << endl;
}
}
and here is a clip of the output:
129, 150
robot::right_link5_collision, robot::right_link5_collision
129, 152
robot::right_link5_collision, robot::right_link5_collision
129, 154
robot::right_link5_collision, robot::right_link5_collision
129, 156
robot::right_link5_collision, robot::right_link5_collision
129, 162
robot::right_link5_collision, robot::right_link5_collision
129, 168
robot::right_link5_collision, robot::right_link5_collision
Here is the corresponding part of the SDF file that contains right_link5 (defined only once):
<link name='right_link5'>
<pose relative_to='right_joint5'>0 0 0 0 -0 0</pose>
<inertial>
<pose>-0.000234 0.036705 -0.080064 0 -0 0</pose>
<mass>0.18554</mass>
<inertia>
<ixx>0.010000</ixx>
<ixy>8.9957e-07</ixy>
<ixz>8.5285e-07</ixz>
<iyy>0.010000</iyy>
<iyz>-0.00025682</iyz>
<izz>0.010000</izz>
</inertia>
</inertial>
<collision name='right_link5_collision'>
<pose>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>file:///home/user/.../collision/link5.STL</uri>
</mesh>
</geometry>
</collision>
<visual name='right_link5_visual'>
<pose>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>file:///home/user/.../visual/link5.STL</uri>
</mesh>
</geometry>
</visual>
</link>
Any thoughts on why this could be happening? I also printed out more information, and these geometry bodies seem to be identical:
129, 162
robot::right_link5_collision, robot::right_link5_collision
Left:
frame_id: 125
geom_model_index: 2
robot_model_index: 2
source_name: plant
source_id: 15
Right:
frame_id: 125
geom_model_index: 2
robot_model_index: 2
source_name: plant
source_id: 15
Related
I use following code to generate discrete dynamic gradient matrix. It looks like to generate different value in some matrix element when runs the same get_dynamics_gradient2() for several times. I can't find any obvious mistake from many testing. Could you tell me how to get it correct?
#include "drake/common/find_resource.h"
#include "drake/multibody/parsing/parser.h"
#include "drake/multibody/plant/multibody_plant.h"
#include "drake/systems/framework/diagram_builder.h"
#include <Eigen/Dense>
#include "drake/math/autodiff.h"
#include "drake/math/autodiff_gradient.h"
#include "iostream"
using namespace std;
using drake::FindResourceOrThrow;
using drake::multibody::MultibodyPlant;
using drake::multibody::Parser;
using drake::systems::Context;
using drake::systems::InputPort;
using drake::systems::DiagramBuilder;
using Eigen::VectorXd;
using drake::AutoDiffVecXd;
using Eigen::MatrixXd;
using drake::AutoDiffXd;
using drake::math::initializeAutoDiff;
using drake::math::autoDiffToGradientMatrix;
using drake::math::autoDiffToValueMatrix;
MatrixXd get_dynamics_gradient2(std::unique_ptr<MultibodyPlant<AutoDiffXd>>& plant_ad,std::unique_ptr<Context<AutoDiffXd>>& context_ad,const VectorXd& x_val,const VectorXd& u_val) {
AutoDiffVecXd x_ad = initializeAutoDiff(x_val);
context_ad -> SetContinuousState(x_ad);
AutoDiffVecXd u_ad = initializeAutoDiff(u_val);
const InputPort<Eigen::AutoDiffScalar<Eigen::VectorXd>>& actuation_port = plant_ad -> get_actuation_input_port();
actuation_port.FixValue(context_ad.get(), u_ad);
auto derivatives = plant_ad -> AllocateTimeDerivatives();
plant_ad -> CalcTimeDerivatives(*context_ad, derivatives.get());
AutoDiffVecXd xdot_ad = derivatives -> get_vector().CopyToVector();
AutoDiffVecXd x_next_ad = xdot_ad * 0.1 + x_ad;
MatrixXd x_next = autoDiffToValueMatrix(x_next_ad);
AutoDiffVecXd x_next_ad_t = x_next_ad.transpose();
AutoDiffVecXd u_ad_t = u_ad.transpose();
AutoDiffVecXd xu_next_ad_t(x_next_ad_t.rows()+u_ad_t.rows(),x_next_ad_t.cols());
xu_next_ad_t << x_next_ad_t,u_ad_t;
MatrixXd AB = autoDiffToGradientMatrix(xu_next_ad_t.transpose());
return AB;
}
int main(int argc, char* argv[]) {
const double time_step = 0;
DiagramBuilder<double> builder;
const std::string relative_name = "drake/wc/ll0/acrobot.sdf";
const std::string full_name = FindResourceOrThrow(relative_name);
MultibodyPlant<double>& acrobot = *builder.AddSystem<MultibodyPlant>(time_step);
Parser parser(&acrobot);
parser.AddModelFromFile(full_name);
acrobot.Finalize();
std::unique_ptr<MultibodyPlant<AutoDiffXd>> plant_ad = MultibodyPlant<double>::ToAutoDiffXd(acrobot);
std::unique_ptr<Context<AutoDiffXd>> context_ad = plant_ad -> CreateDefaultContext();
VectorXd x_valp = VectorXd(4);
x_valp << 1.3, 5.8, 1.5, 0.02;
VectorXd u_valp= VectorXd(1);
u_valp << 0.01;
MatrixXd AB1 = get_dynamics_gradient2(plant_ad, context_ad, x_valp, u_valp);
MatrixXd AB2 = get_dynamics_gradient2(plant_ad, context_ad, x_valp, u_valp);
MatrixXd AB3 = get_dynamics_gradient2(plant_ad, context_ad, x_valp, u_valp);
MatrixXd AB4 = get_dynamics_gradient2(plant_ad, context_ad, x_valp, u_valp);
cout <<"AB2-AB1" << endl << AB2-AB1 <<endl;
cout <<"AB3-AB1" << endl << AB3-AB1 <<endl;
cout <<"AB4-AB1" << endl << AB4-AB1 <<endl;
return 0;
}
Here is the result:
AB2-AB1
0 0 0 0
0 0 0 0
0 0 0.735961 0.377986
0 0 -1.47292 -0.756483
0 1.73059e-77 1.96356 0
AB3-AB1
0 0 0 0
0 0 0 0
0 0 -0.35439 -0.35439
0 0 0.70926 0.70926
0 1.73059e-77 0.963558 0
AB4-AB1
0 0 0 0
0 0 0 0
0 0 1.28067 1.50475
0 0 -2.56308 -3.01153
0 2.90227e-157 0.963558 0
I'm trying to extract the 3x3 rotation matrix from the 3x4 pose matrix I have. However, two values are differing even though I have very simple code setting one to the other. I'm banging my head against the wall because I have no idea why this is happening. Here is the code:
std::cout << "Camera pose matrix from optical flow homography" << std::endl;
for (int e = 0; e < pose.rows; e++) {
for (int f = 0; f < pose.cols; f++) {
std::cout << pose.at<double>(e,f) << " " << e << " " << f;
std::cout << " ";
}
std::cout << "\n" << std::endl;
}
std::cout << "Creating rotation matrix" << std::endl;
Mat rotvec = Mat::eye(3, 3, CV_32FC1);
for (int s = 0; s < pose.rows; s++) {
for (int g = 0; g < pose.cols-1; g++) {
rotvec.at<double>(s, g) = pose.at<double>(s,g);
std::cout << rotvec.at<double>(s,g) << " " << s << " " << g;
std::cout << " ";
}
std::cout << "\n" << std::endl;
}
std::cout << "Rotation matrix" << std::endl;
for (int e = 0; e < pose.rows; e++) {
for (int f = 0; f < pose.cols-1; f++) {
std::cout << rotvec.at<double>(e,f) << " " << e << " " << f;
std::cout << " ";
std::cout << pose.at<double>(e,f) << " " << e << " " << f;
std::cout << " ";
}
std::cout << "\n" << std::endl;
}
Here is the output:
Camera pose matrix from optical flow homography
5.26354e-315 0 0 0 0 1 0.0078125 0 2 0 0 3
0.0078125 1 0 0 1 1 0 1 2 5.26354e-315 1 3
0 2 0 5.26354e-315 2 1 1.97626e-323 2 2 7.64868e-309 2 3
Creating rotation matrix
5.26354e-315 0 0 0 0 1 0.0078125 0 2
0.0078125 1 0 0 1 1 0 1 2
0 2 0 5.26354e-315 2 1 1.97626e-323 2 2
Rotation matrix
5.26354e-315 0 0 5.26354e-315 0 0 0 0 1 0 0 1 5.26354e-315 0 2 0.0078125 0 2
0.0078125 1 0 0.0078125 1 0 0 1 1 0 1 1 0.0078125 1 2 0 1 2
0 2 0 0 2 0 5.26354e-315 2 1 5.26354e-315 2 1 1.97626e-323 2 2 1.97626e-323 2 2
Here you can see I'm trying to save the first three columns of pose into the rotvec matrix. When I actually set the rotation matrix equal to the pose for those three columns, I get the correct matrix, as the second matrix is equal to the first three columns of the first matrix. However, when I check the rotation matrix once again, (third matrix) it is not the same as the output I require on coordinates (0, 2) and (1, 2). (I outputted the rotvec matrix number next to the pose matrix number, and you can see at these coordinates the numbers do not match). I am not sure why this is happening, could someone please help me out?
Solved my problem for anyone else who stumbles upon this later: I just changed the Mat type to CV_64F (to make it double) for both rotvec and pose, and used all to display. Creds to berak for pointing me in the right direction.
The following code gives inconsistent covariance matrix sizes.
cv::Mat A = (cv::Mat_<float>(3,2) << -1, 1, -2, 3, 4, 0);
cv::Mat covar1, covar2, covar3, covar4, mean;
calcCovarMatrix(A, covar1, mean, CV_COVAR_NORMAL | CV_COVAR_ROWS);
calcCovarMatrix(A, covar2, mean, CV_COVAR_SCRAMBLED | CV_COVAR_ROWS);
calcCovarMatrix(A, covar3, mean, CV_COVAR_NORMAL | CV_COVAR_COLS);
calcCovarMatrix(A, covar4, mean, CV_COVAR_SCRAMBLED | CV_COVAR_COLS);
std::cout << "size: " << covar1.size() << "\n";
std::cout << "size: " << covar2.size() << "\n";
std::cout << "size: " << covar3.size() << "\n";
std::cout << "size: " << covar4.size() << "\n";
covar1 and covar2 should have the same size because they both describe the covariance over the rows, and covar3 and covar4 should have the same size because they both describe the covariance over the columns, respectively. However, the output is:
size: [2 x 2]
size: [3 x 3]
size: [3 x 3]
size: [2 x 2]
The calcCovarMatrix() docs, specifically say that when using CV_COVAR_SCRAMBLED "The covariance matrix will be nsamples x nsamples."
SSE intrinsics includes _mm_shuffle_ps xmm1 xmm2 immx which allows one to pick 2 elements from xmm1 concatenated with 2 elements from xmm2. However this is for floats, (implied by the _ps , packed single). However if you cast your packed integers __m128i, then you can use _mm_shuffle_ps as well:
#include <iostream>
#include <immintrin.h>
#include <sstream>
using namespace std;
template <typename T>
std::string __m128i_toString(const __m128i var) {
std::stringstream sstr;
const T* values = (const T*) &var;
if (sizeof(T) == 1) {
for (unsigned int i = 0; i < sizeof(__m128i); i++) {
sstr << (int) values[i] << " ";
}
} else {
for (unsigned int i = 0; i < sizeof(__m128i) / sizeof(T); i++) {
sstr << values[i] << " ";
}
}
return sstr.str();
}
int main(){
cout << "Starting SSE test" << endl;
cout << "integer shuffle" << endl;
int A[] = {1, -2147483648, 3, 5};
int B[] = {4, 6, 7, 8};
__m128i pC;
__m128i* pA = (__m128i*) A;
__m128i* pB = (__m128i*) B;
*pA = (__m128i)_mm_shuffle_ps((__m128)*pA, (__m128)*pB, _MM_SHUFFLE(3, 2, 1 ,0));
pC = _mm_add_epi32(*pA,*pB);
cout << "A[0] = " << A[0] << endl;
cout << "A[1] = " << A[1] << endl;
cout << "A[2] = " << A[2] << endl;
cout << "A[3] = " << A[3] << endl;
cout << "B[0] = " << B[0] << endl;
cout << "B[1] = " << B[1] << endl;
cout << "B[2] = " << B[2] << endl;
cout << "B[3] = " << B[3] << endl;
cout << "pA = " << __m128i_toString<int>(*pA) << endl;
cout << "pC = " << __m128i_toString<int>(pC) << endl;
}
Snippet of relevant corresponding assembly (mac osx, macports gcc 4.8, -march=native on an ivybridge CPU):
vshufps $228, 16(%rsp), %xmm1, %xmm0
vpaddd 16(%rsp), %xmm0, %xmm2
vmovdqa %xmm0, 32(%rsp)
vmovaps %xmm0, (%rsp)
vmovdqa %xmm2, 16(%rsp)
call __ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc
....
Thus it seemingly works fine on integers, which I expected as the registers are agnostic to types, however there must be a reason why the docs say that this instruction is only for floats. Does someone know any downsides, or implications I have missed?
There is no equivalent to _mm_shuffle_ps for integers. To achieve the same effect in this case you can do
SSE2
*pA = _mm_shuffle_epi32(_mm_unpacklo_epi32(*pA, _mm_shuffle_epi32(*pB, 0xe)),0xd8);
SSE4.1
*pA = _mm_blend_epi16(*pA, *pB, 0xf0);
or change to the floating point domain like this
*pA = _mm_castps_si128(
_mm_shuffle_ps(_mm_castsi128_ps(*pA),
_mm_castsi128_ps(*pB), _MM_SHUFFLE(3, 2, 1 ,0)));
But changing domains may incur bypass latency delays on some CPUs. Keep in mind that according to Agner
The bypass delay is important in long dependency chains where latency is a bottleneck, but
not where it is throughput rather than latency that matters.
You have to test your code and see which method above is more efficient.
Fortunately, on most Intel/AMD CPUs, there is usually no penalty for using shufps between most integer-vector instructions. Agner says:
For example, I found no delay when mixing PADDD and SHUFPS [on Sandybridge].
Nehalem does have 2 bypass-delay latency to/from SHUFPS, but even then a single SHUFPS is often still faster than multiple other instructions. Extra instructions have latency, too, as well as costing throughput.
The reverse (integer shuffles between FP math instructions) is not as safe:
In Agner Fog's microarchitecture on page 112 in Example 8.3a, he shows that using PSHUFD (_mm_shuffle_epi32) instead of SHUFPS (_mm_shuffle_ps) when in the floating point domain causes a bypass delay of four clock cycles. In Example 8.3b he uses SHUFPS to remove the delay (which works in his example).
On Nehalem there are actually five domains. Nahalem seems to be the most effected (the bypass delays did not exist before Nahalem). On Sandy Bridge the delays are less significant. This is even more true on Haswell. In fact on Haswell Agner said he found no delays between SHUFPS or PSHUFD (see page 140).
I'm writing an iOS app that uses CoreAudio's Audio Unit API, and at a certain point I do a AudioUnitGetProperty(audioUnit, kAudioUnitProperty_StreamFormat, ...) call. At that point I set a breakpoint to look into the ASBD, and I find the mFormatFlags field is 41. The question is, can I decode the actual flag names from that number (such as kAudioFormatFlagIsNonInterleaved | kAudioFormatFlagIsPacked | ...)?
Thanks a lot.
Here is an ostream overload for printing an ASBD, taken mostly from Apple sample code:
std::ostream& operator<<(std::ostream& out, const AudioStreamBasicDescription& format)
{
unsigned char formatID [5];
*(UInt32 *)formatID = OSSwapHostToBigInt32(format.mFormatID);
formatID[4] = '\0';
// General description
out << format.mChannelsPerFrame << " ch, " << format.mSampleRate << " Hz, '" << formatID << "' (0x" << std::hex << std::setw(8) << std::setfill('0') << format.mFormatFlags << std::dec << ") ";
if(kAudioFormatLinearPCM == format.mFormatID) {
// Bit depth
UInt32 fractionalBits = ((0x3f << 7)/*kLinearPCMFormatFlagsSampleFractionMask*/ & format.mFormatFlags) >> 7/*kLinearPCMFormatFlagsSampleFractionShift*/;
if(0 < fractionalBits)
out << (format.mBitsPerChannel - fractionalBits) << "." << fractionalBits;
else
out << format.mBitsPerChannel;
out << "-bit";
// Endianness
bool isInterleaved = !(kAudioFormatFlagIsNonInterleaved & format.mFormatFlags);
UInt32 interleavedChannelCount = (isInterleaved ? format.mChannelsPerFrame : 1);
UInt32 sampleSize = (0 < format.mBytesPerFrame && 0 < interleavedChannelCount ? format.mBytesPerFrame / interleavedChannelCount : 0);
if(1 < sampleSize)
out << ((kLinearPCMFormatFlagIsBigEndian & format.mFormatFlags) ? " big-endian" : " little-endian");
// Sign
bool isInteger = !(kLinearPCMFormatFlagIsFloat & format.mFormatFlags);
if(isInteger)
out << ((kLinearPCMFormatFlagIsSignedInteger & format.mFormatFlags) ? " signed" : " unsigned");
// Integer or floating
out << (isInteger ? " integer" : " float");
// Packedness
if(0 < sampleSize && ((sampleSize << 3) != format.mBitsPerChannel))
out << ((kLinearPCMFormatFlagIsPacked & format.mFormatFlags) ? ", packed in " : ", unpacked in ") << sampleSize << " bytes";
// Alignment
if((0 < sampleSize && ((sampleSize << 3) != format.mBitsPerChannel)) || (0 != (format.mBitsPerChannel & 7)))
out << ((kLinearPCMFormatFlagIsAlignedHigh & format.mFormatFlags) ? " high-aligned" : " low-aligned");
if(!isInterleaved)
out << ", deinterleaved";
}
else if(kAudioFormatAppleLossless == format.mFormatID) {
UInt32 sourceBitDepth = 0;
switch(format.mFormatFlags) {
case kAppleLosslessFormatFlag_16BitSourceData: sourceBitDepth = 16; break;
case kAppleLosslessFormatFlag_20BitSourceData: sourceBitDepth = 20; break;
case kAppleLosslessFormatFlag_24BitSourceData: sourceBitDepth = 24; break;
case kAppleLosslessFormatFlag_32BitSourceData: sourceBitDepth = 32; break;
}
if(0 != sourceBitDepth)
out << "from " << sourceBitDepth << "-bit source, ";
else
out << "from UNKNOWN source bit depth, ";
out << format.mFramesPerPacket << " frames/packet";
}
else
out << format.mBitsPerChannel << " bits/channel, " << format.mBytesPerPacket << " bytes/packet, " << format.mFramesPerPacket << " frames/packet, " << format.mBytesPerFrame << " bytes/frame";
return out;
}