Skip to content

Commit f32ff21

Browse files
brendanburkhartbrendanburkhart
authored
Merge pull request #112 from jhu-cisst/feature-cartesian-state
Cartesian state/servo parameter types
2 parents f293e5b + 31f7f04 commit f32ff21

File tree

8 files changed

+398
-0
lines changed

8 files changed

+398
-0
lines changed

‎cisstParameterTypes/cisstParameterTypes.i

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -84,6 +84,10 @@ http://www.cisst.org/cisst/license.txt.
8484
%include "cisstParameterTypes/prmOperatingState.h"
8585
%include "cisstParameterTypes/prmActuatorState.h"
8686

87+
%include "cisstParameterTypes/prmStateCartesian.h"
88+
%include "cisstParameterTypes/prmServoCartesian.h"
89+
%include "cisstParameterTypes/prmServoJoint.h"
90+
8791
%include "cisstParameterTypes/prmImageFrame.h"
8892
%include "cisstParameterTypes/prmCameraInfo.h"
8993
%include "cisstParameterTypes/prmDepthMap.h"

‎cisstParameterTypes/code/CMakeLists.txt

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,9 @@ cisst_data_generator (cisstParameterTypes
2626
../prmCartesianImpedance.cdg
2727
../prmIMUSensors.cdg
2828
../prmStateJoint.cdg
29+
../prmStateCartesian.cdg
30+
../prmServoJoint.cdg
31+
../prmServoCartesian.cdg
2932
../prmConfigurationJoint.cdg
3033
../prmInputData.cdg
3134
../prmKeyValue.cdg
@@ -87,6 +90,8 @@ set (HEADER_FILES
8790

8891
prmJointType.h
8992
prmInputDataConverter.h
93+
94+
prmSetpointMode.h
9095
)
9196

9297
cisst_add_library (

‎cisstParameterTypes/code/prmClassServices.cpp

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -93,6 +93,15 @@ CMN_IMPLEMENT_SERVICES(prmConfigurationJoint);
9393
#include <cisstParameterTypes/prmStateJoint.h>
9494
CMN_IMPLEMENT_SERVICES(prmStateJoint);
9595

96+
#include <cisstParameterTypes/prmStateCartesian.h>
97+
CMN_IMPLEMENT_SERVICES(prmStateCartesian);
98+
99+
#include <cisstParameterTypes/prmServoJoint.h>
100+
CMN_IMPLEMENT_SERVICES(prmServoJoint);
101+
102+
#include <cisstParameterTypes/prmServoCartesian.h>
103+
CMN_IMPLEMENT_SERVICES(prmServoCartesian);
104+
96105
#include <cisstParameterTypes/prmInputData.h>
97106
CMN_IMPLEMENT_SERVICES(prmInputData);
98107

‎cisstParameterTypes/prmPython.h

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -58,6 +58,10 @@ no warranty. The complete license can be found in license.txt and
5858
#include <cisstParameterTypes/prmOperatingState.h>
5959
#include <cisstParameterTypes/prmActuatorState.h>
6060

61+
#include <cisstParameterTypes/prmStateCartesian.h>
62+
#include <cisstParameterTypes/prmServoCartesian.h>
63+
#include <cisstParameterTypes/prmServoJoint.h>
64+
6165
#include <cisstParameterTypes/prmImageFrame.h>
6266
#include <cisstParameterTypes/prmCameraInfo.h>
6367
#include <cisstParameterTypes/prmDepthMap.h>

‎cisstParameterTypes/prmServoCartesian.cdg

Lines changed: 62 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,62 @@
1+
// -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*-
2+
// ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab:
3+
4+
inline-header {
5+
#include <cisstVector/vctFixedSizeVectorTypes.h>
6+
#include <cisstVector/vctTransformationTypes.h>
7+
#include <cisstMultiTask/mtsGenericObject.h>
8+
#include <cisstParameterTypes/prmSetpointMode.h>
9+
10+
// Always include last
11+
#include <cisstParameterTypes/prmExport.h>
12+
}
13+
14+
class {
15+
name prmServoCartesian;
16+
17+
attribute CISST_EXPORT;
18+
19+
base-class {
20+
type mtsGenericObject;
21+
is-data true;
22+
}
23+
24+
member {
25+
name TaskFrame;
26+
type vctFrm3;
27+
default vctFrm3(); // identity
28+
}
29+
30+
member {
31+
name Position;
32+
type vctFrm3;
33+
default vctFrm3(); // identity
34+
}
35+
36+
member {
37+
name Velocity;
38+
type vct6;
39+
default vct6(0.0);
40+
}
41+
42+
member {
43+
name Force;
44+
type vct6;
45+
default vct6(0.0);
46+
}
47+
48+
member {
49+
name AxisMode;
50+
type vctFixedSizeVector<prmSetpointMode, 6>;
51+
default vctFixedSizeVector<prmSetpointMode, 6>(prmSetpointMode::NONE, prmSetpointMode::NONE, prmSetpointMode::NONE, prmSetpointMode::NONE, prmSetpointMode::NONE, prmSetpointMode::NONE);
52+
}
53+
54+
inline-header {
55+
private:
56+
CMN_DECLARE_SERVICES(CMN_DYNAMIC_CREATION, CMN_LOG_ALLOW_DEFAULT);
57+
}
58+
}
59+
60+
inline-header {
61+
CMN_DECLARE_SERVICES_INSTANTIATION(prmServoCartesian);
62+
}

‎cisstParameterTypes/prmServoJoint.cdg

Lines changed: 78 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,78 @@
1+
// -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*-
2+
// ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab:
3+
4+
inline-header {
5+
#include <cisstVector/vctFixedSizeVectorTypes.h>
6+
#include <cisstVector/vctTransformationTypes.h>
7+
#include <cisstMultiTask/mtsGenericObject.h>
8+
#include <cisstParameterTypes/prmSetpointMode.h>
9+
10+
// Always include last
11+
#include <cisstParameterTypes/prmExport.h>
12+
}
13+
14+
class {
15+
name prmServoJoint;
16+
17+
attribute CISST_EXPORT;
18+
19+
base-class {
20+
type mtsGenericObject;
21+
is-data true;
22+
}
23+
24+
member {
25+
name Name;
26+
type std::vector<std::string>;
27+
default std::vector<std::string>();
28+
}
29+
30+
member {
31+
name Mode;
32+
type std::vector<prmSetpointMode>;
33+
default std::vector<prmSetpointMode>(0);
34+
}
35+
36+
member {
37+
name Position;
38+
type vctDoubleVec;
39+
default vctDoubleVec(0);
40+
}
41+
42+
member {
43+
name Velocity;
44+
type vctDoubleVec;
45+
default vctDoubleVec(0);
46+
}
47+
48+
member {
49+
name Effort;
50+
type vctDoubleVec;
51+
default vctDoubleVec(0);
52+
}
53+
54+
member {
55+
name PositionProjection;
56+
type vctDoubleMat;
57+
default vctDoubleMat(0, 0, 0.0);
58+
description If size is 0, will be treated as if identity.;
59+
}
60+
61+
inline-header {
62+
public:
63+
inline void SetSize(const size_t & size) {
64+
Name().resize(size);
65+
Mode().resize(size);
66+
Position().SetSize(size, 0.0);
67+
Velocity().SetSize(size, 0.0);
68+
Effort().SetSize(size, 0.0);
69+
PositionProjection().SetSize(size, size, 0.0);
70+
}
71+
private:
72+
CMN_DECLARE_SERVICES(CMN_DYNAMIC_CREATION, CMN_LOG_ALLOW_DEFAULT);
73+
}
74+
}
75+
76+
inline-header {
77+
CMN_DECLARE_SERVICES_INSTANTIATION(prmServoJoint);
78+
}

‎cisstParameterTypes/prmSetpointMode.h

Lines changed: 154 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,154 @@
1+
// -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*-
2+
// ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab:
3+
4+
#ifndef _prmSetpointMode_h
5+
#define _prmSetpointMode_h
6+
7+
#include <cstdint>
8+
9+
// Always include last
10+
#include <cisstParameterTypes/prmExport.h>
11+
12+
enum class prmSetpointMode : uint8_t {
13+
NONE = 0,
14+
POSITION = 1 << 0,
15+
VELOCITY = 1 << 1,
16+
EFFORT = 1 << 2
17+
};
18+
19+
// operator to combine setpoint modes
20+
inline prmSetpointMode operator|(prmSetpointMode a, prmSetpointMode b)
21+
{
22+
return static_cast<prmSetpointMode>(static_cast<uint8_t>(a) | static_cast<uint8_t>(b));
23+
}
24+
25+
// operator to test setpoint modes
26+
inline bool operator&(prmSetpointMode a, prmSetpointMode b)
27+
{
28+
return static_cast<bool>(static_cast<uint8_t>(a) & static_cast<uint8_t>(b));
29+
}
30+
31+
inline std::ostream & operator << (std::ostream & output,
32+
const prmSetpointMode & mode) {
33+
std::string mode_name = "";
34+
if (mode & prmSetpointMode::POSITION) {
35+
mode_name += "P";
36+
}
37+
if (mode & prmSetpointMode::VELOCITY) {
38+
mode_name += "V";
39+
}
40+
if (mode & prmSetpointMode::EFFORT) {
41+
mode_name += "F";
42+
}
43+
44+
if (mode_name == "") {
45+
mode_name = "NONE";
46+
}
47+
48+
return output << mode_name;
49+
}
50+
51+
template <>
52+
inline void CISST_EXPORT cmnDataJSON<prmSetpointMode>::SerializeText(const DataType & data, Json::Value & jsonValue) {
53+
jsonValue = static_cast<uint8_t>(data);
54+
}
55+
56+
template <>
57+
inline void CISST_EXPORT cmnDataJSON<prmSetpointMode>::DeSerializeText(DataType & data, const Json::Value & jsonValue) CISST_THROW(std::runtime_error) {
58+
if (jsonValue.empty()) {
59+
cmnThrow("cmnDataJSON<prmSetpointMode>::DeSerializeText: empty JSON value");
60+
}
61+
62+
// check that value can be expressed as bitwise-or of known setpoint modes
63+
unsigned int raw_data = jsonValue.asUInt();
64+
constexpr size_t num_nonzero_modes = 3;
65+
if (raw_data >= (1 << num_nonzero_modes)) {
66+
cmnThrow("cmnDataJSON<prmSetpointMode>::DeSerializeText: JSON value does not correspond to combination of known modes");
67+
}
68+
69+
data = static_cast<prmSetpointMode>(raw_data);
70+
}
71+
72+
template <>
73+
class cmnData<prmSetpointMode> {
74+
public:
75+
enum {IS_SPECIALIZED = 1};
76+
77+
static void Copy(prmSetpointMode & data, const prmSetpointMode & source) {
78+
data = source;
79+
}
80+
81+
static std::string HumanReadable(const prmSetpointMode & data) {
82+
switch (data)
83+
{
84+
case prmSetpointMode::NONE:
85+
return "prmSetpointMode::NONE";
86+
case prmSetpointMode::POSITION:
87+
return "prmSetpointMode::POSITION";
88+
case prmSetpointMode::VELOCITY:
89+
return "prmSetpointMode::VELOCITY";
90+
case prmSetpointMode::EFFORT:
91+
return "prmSetpointMode::EFFORT";
92+
default:
93+
return "prmSetpointMode unknown";
94+
}
95+
}
96+
97+
static void SerializeBinary(const prmSetpointMode & data,
98+
std::ostream & outputStream) CISST_THROW(std::runtime_error) {
99+
cmnData<uint8_t>::SerializeBinary(static_cast<uint8_t>(data), outputStream);
100+
}
101+
102+
static void DeSerializeBinary(prmSetpointMode & data, std::istream & inputStream,
103+
const cmnDataFormat & localFormat,
104+
const cmnDataFormat & remoteFormat) CISST_THROW(std::runtime_error) {
105+
uint8_t value;
106+
cmnData<uint8_t>::DeSerializeBinary(value, inputStream, localFormat, remoteFormat);
107+
data = static_cast<prmSetpointMode>(value);
108+
}
109+
110+
static void SerializeText(const prmSetpointMode & data, std::ostream & outputStream,
111+
const char CMN_UNUSED(delimiter) = ',') CISST_THROW(std::runtime_error) {
112+
outputStream << static_cast<uint8_t>(data);
113+
}
114+
115+
static std::string SerializeDescription(const prmSetpointMode & CMN_UNUSED(data),
116+
const char CMN_UNUSED(delimiter) = ',',
117+
const std::string & userDescription = "") {
118+
return (userDescription == "") ? "prmSetpointMode" : (userDescription + "prmSetpointMode");
119+
}
120+
121+
static void DeSerializeText(prmSetpointMode & data, std::istream & inputStream,
122+
const char CMN_UNUSED(delimiter) = ',') CISST_THROW(std::runtime_error) {
123+
uint8_t value;
124+
inputStream >> value;
125+
data = static_cast<prmSetpointMode>(value);
126+
}
127+
128+
static bool ScalarNumberIsFixed(const prmSetpointMode & CMN_UNUSED(data)) {
129+
return true;
130+
}
131+
132+
static size_t ScalarNumber(const prmSetpointMode & CMN_UNUSED(data)) {
133+
return 1;
134+
}
135+
136+
static std::string ScalarDescription(const prmSetpointMode & CMN_UNUSED(data), const size_t index,
137+
const std::string & userDescription) CISST_THROW(std::out_of_range) {
138+
if (index == 0) {
139+
return (userDescription == "") ? "prmSetpointMode" : (userDescription + "prmSetpointMode");
140+
}
141+
cmnThrow(std::out_of_range("cmnDataScalarDescription: prmSetpointMode index out of range"));
142+
return "";
143+
}
144+
145+
static double Scalar(const prmSetpointMode & data, const size_t index) CISST_THROW(std::out_of_range) {
146+
if (index == 0) {
147+
return static_cast<double>(data);
148+
}
149+
cmnThrow(std::out_of_range("cmnDataScalar: prmSetpointMode index out of range"));
150+
return 0.0;
151+
}
152+
};
153+
154+
#endif // _prmSetpointMode_h

0 commit comments

Comments
 (0)