fix some typos
This commit is contained in:
parent
17476b25dc
commit
81a6d4f4cc
|
@ -851,7 +851,7 @@ void MeshObject::offsetSpecial2(float fSize)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// if there no flipped triangels -> stop
|
// if there are no flipped triangles -> stop
|
||||||
//int f =fliped.size();
|
//int f =fliped.size();
|
||||||
if (fliped.size() == 0)
|
if (fliped.size() == 0)
|
||||||
break;
|
break;
|
||||||
|
|
|
@ -98,7 +98,7 @@ void MeshAlgos::offsetSpecial2(MeshCore::MeshKernel* Mesh, float fSize)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// if there no flipped triangels -> stop
|
// if there are no flipped triangles -> stop
|
||||||
//int f =fliped.size();
|
//int f =fliped.size();
|
||||||
if(fliped.size() == 0)
|
if(fliped.size() == 0)
|
||||||
break;
|
break;
|
||||||
|
|
|
@ -215,7 +215,7 @@ PROPERTY_SOURCE(PartDesign::Cylinder, PartDesign::FeaturePrimitive)
|
||||||
Cylinder::Cylinder()
|
Cylinder::Cylinder()
|
||||||
{
|
{
|
||||||
ADD_PROPERTY_TYPE(Radius,(10.0f),"Cylinder",App::Prop_None,"The radius of the cylinder");
|
ADD_PROPERTY_TYPE(Radius,(10.0f),"Cylinder",App::Prop_None,"The radius of the cylinder");
|
||||||
ADD_PROPERTY_TYPE(Angle,(360.0f),"Cylinder",App::Prop_None,"The closing angel of the cylinder ");
|
ADD_PROPERTY_TYPE(Angle,(360.0f),"Cylinder",App::Prop_None,"The closing angle of the cylinder ");
|
||||||
ADD_PROPERTY_TYPE(Height,(10.0f),"Cylinder",App::Prop_None,"The height of the cylinder");
|
ADD_PROPERTY_TYPE(Height,(10.0f),"Cylinder",App::Prop_None,"The height of the cylinder");
|
||||||
Angle.setConstraints(&angleRangeU);
|
Angle.setConstraints(&angleRangeU);
|
||||||
Radius.setConstraints(&quantityRange);
|
Radius.setConstraints(&quantityRange);
|
||||||
|
|
|
@ -33,11 +33,11 @@
|
||||||
#include "kdl_cp/chainfksolver.hpp"
|
#include "kdl_cp/chainfksolver.hpp"
|
||||||
#include "kdl_cp/chainfksolverpos_recursive.hpp"
|
#include "kdl_cp/chainfksolverpos_recursive.hpp"
|
||||||
#include "kdl_cp/frames_io.hpp"
|
#include "kdl_cp/frames_io.hpp"
|
||||||
#include "kdl_cp/chainiksolver.hpp"
|
#include "kdl_cp/chainiksolver.hpp"
|
||||||
#include "kdl_cp/chainiksolvervel_pinv.hpp"
|
#include "kdl_cp/chainiksolvervel_pinv.hpp"
|
||||||
#include "kdl_cp/chainjnttojacsolver.hpp"
|
#include "kdl_cp/chainjnttojacsolver.hpp"
|
||||||
#include "kdl_cp/chainiksolverpos_nr.hpp"
|
#include "kdl_cp/chainiksolverpos_nr.hpp"
|
||||||
#include "kdl_cp/chainiksolverpos_nr_jl.hpp"
|
#include "kdl_cp/chainiksolverpos_nr_jl.hpp"
|
||||||
|
|
||||||
#include "Robot6Axis.h"
|
#include "Robot6Axis.h"
|
||||||
#include "RobotAlgos.h"
|
#include "RobotAlgos.h"
|
||||||
|
@ -53,28 +53,28 @@
|
||||||
using namespace Robot;
|
using namespace Robot;
|
||||||
using namespace Base;
|
using namespace Base;
|
||||||
using namespace KDL;
|
using namespace KDL;
|
||||||
|
|
||||||
// some default roboter
|
// some default roboter
|
||||||
|
|
||||||
AxisDefinition KukaIR500[6] = {
|
AxisDefinition KukaIR500[6] = {
|
||||||
// a ,alpha ,d ,theta ,rotDir ,maxAngle ,minAngle ,AxisVelocity
|
// a ,alpha ,d ,theta ,rotDir ,maxAngle ,minAngle ,AxisVelocity
|
||||||
{500 ,-90 ,1045 ,0 , -1 ,+185 ,-185 ,156 }, // Axis 1
|
{500 ,-90 ,1045 ,0 , -1 ,+185 ,-185 ,156 }, // Axis 1
|
||||||
{1300 ,0 ,0 ,0 , 1 ,+35 ,-155 ,156 }, // Axis 2
|
{1300 ,0 ,0 ,0 , 1 ,+35 ,-155 ,156 }, // Axis 2
|
||||||
{55 ,+90 ,0 ,-90 , 1 ,+154 ,-130 ,156 }, // Axis 3
|
{55 ,+90 ,0 ,-90 , 1 ,+154 ,-130 ,156 }, // Axis 3
|
||||||
{0 ,-90 ,-1025,0 , 1 ,+350 ,-350 ,330 }, // Axis 4
|
{0 ,-90 ,-1025,0 , 1 ,+350 ,-350 ,330 }, // Axis 4
|
||||||
{0 ,+90 ,0 ,0 , 1 ,+130 ,-130 ,330 }, // Axis 5
|
{0 ,+90 ,0 ,0 , 1 ,+130 ,-130 ,330 }, // Axis 5
|
||||||
{0 ,+180 ,-300 ,0 , 1 ,+350 ,-350 ,615 } // Axis 6
|
{0 ,+180 ,-300 ,0 , 1 ,+350 ,-350 ,615 } // Axis 6
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
TYPESYSTEM_SOURCE(Robot::Robot6Axis , Base::Persistence);
|
TYPESYSTEM_SOURCE(Robot::Robot6Axis , Base::Persistence);
|
||||||
|
|
||||||
Robot6Axis::Robot6Axis()
|
Robot6Axis::Robot6Axis()
|
||||||
{
|
{
|
||||||
// create joint array for the min and max angel values of each joint
|
// create joint array for the min and max angle values of each joint
|
||||||
Min = JntArray(6);
|
Min = JntArray(6);
|
||||||
Max = JntArray(6);
|
Max = JntArray(6);
|
||||||
|
|
||||||
// Create joint array
|
// Create joint array
|
||||||
Actuall = JntArray(6);
|
Actuall = JntArray(6);
|
||||||
|
|
||||||
|
@ -89,21 +89,21 @@ Robot6Axis::~Robot6Axis()
|
||||||
|
|
||||||
void Robot6Axis::setKinematic(const AxisDefinition KinDef[6])
|
void Robot6Axis::setKinematic(const AxisDefinition KinDef[6])
|
||||||
{
|
{
|
||||||
Chain temp;
|
Chain temp;
|
||||||
|
|
||||||
|
|
||||||
for(int i=0 ; i<6 ;i++){
|
for(int i=0 ; i<6 ;i++){
|
||||||
temp.addSegment(Segment(Joint(Joint::RotZ),Frame::DH(KinDef[i].a ,KinDef[i].alpha * (M_PI/180) ,KinDef[i].d ,KinDef[i].theta * (M_PI/180) )));
|
temp.addSegment(Segment(Joint(Joint::RotZ),Frame::DH(KinDef[i].a ,KinDef[i].alpha * (M_PI/180) ,KinDef[i].d ,KinDef[i].theta * (M_PI/180) )));
|
||||||
RotDir [i] = KinDef[i].rotDir;
|
RotDir [i] = KinDef[i].rotDir;
|
||||||
Max(i) = KinDef[i].maxAngle * (M_PI/180);
|
Max(i) = KinDef[i].maxAngle * (M_PI/180);
|
||||||
Min(i) = KinDef[i].minAngle * (M_PI/180);
|
Min(i) = KinDef[i].minAngle * (M_PI/180);
|
||||||
Velocity[i] = KinDef[i].velocity;
|
Velocity[i] = KinDef[i].velocity;
|
||||||
}
|
}
|
||||||
|
|
||||||
// for now and testing
|
// for now and testing
|
||||||
Kinematic = temp;
|
Kinematic = temp;
|
||||||
|
|
||||||
// get the actuall TCP out of tha axis
|
// get the actuall TCP out of tha axis
|
||||||
calcTcp();
|
calcTcp();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -208,7 +208,7 @@ void Robot6Axis::Restore(XMLReader &reader)
|
||||||
reader.getAttributeAsFloat("Q1"),
|
reader.getAttributeAsFloat("Q1"),
|
||||||
reader.getAttributeAsFloat("Q2"),
|
reader.getAttributeAsFloat("Q2"),
|
||||||
reader.getAttributeAsFloat("Q3")));
|
reader.getAttributeAsFloat("Q3")));
|
||||||
Temp.addSegment(Segment(Joint(Joint::RotZ),toFrame(Tip)));
|
Temp.addSegment(Segment(Joint(Joint::RotZ),toFrame(Tip)));
|
||||||
|
|
||||||
|
|
||||||
if(reader.hasAttribute("rotDir"))
|
if(reader.hasAttribute("rotDir"))
|
||||||
|
|
|
@ -53,7 +53,7 @@
|
||||||
// 2) EulerZYX,EulerZYZ,RPY word followed by a vector, e.g. :
|
// 2) EulerZYX,EulerZYZ,RPY word followed by a vector, e.g. :
|
||||||
// Eulerzyx[10,20,30]
|
// Eulerzyx[10,20,30]
|
||||||
// (ANGLES are always expressed in DEGREES for I/O)
|
// (ANGLES are always expressed in DEGREES for I/O)
|
||||||
// (ANGELS are always expressed in RADIANS for internal representation)
|
// (ANGLES are always expressed in RADIANS for internal representation)
|
||||||
// 3) Rot [1,2,3] [20] Rotates around axis [1,2,3] with an angle
|
// 3) Rot [1,2,3] [20] Rotates around axis [1,2,3] with an angle
|
||||||
// of 20 degrees.
|
// of 20 degrees.
|
||||||
// 4) Identity returns identity rotation matrix.
|
// 4) Identity returns identity rotation matrix.
|
||||||
|
|
Loading…
Reference in New Issue
Block a user