Skip to content

Commit 4320b01

Browse files
costashatzcostashatz
authored
Merge pull request #90 from resibots/robot_misc
Αdd easy limits access and reset in Robot class
2 parents a2cf0c1 + 27b13f6 commit 4320b01

File tree

4 files changed

+279
-15
lines changed

4 files changed

+279
-15
lines changed

‎src/examples/dof_maps.cpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -13,8 +13,8 @@ int main()
1313
{
1414
std::srand(std::time(NULL));
1515

16-
std::vector<std::pair<std::string, std::string>> packages = {{"iiwa14", "iiwa/meshes"}};
17-
auto global_robot = std::make_shared<robot_dart::Robot>("iiwa/iiwa14.urdf", packages);
16+
std::vector<std::pair<std::string, std::string>> packages = {{"iiwa_description", "iiwa/iiwa_description"}};
17+
auto global_robot = std::make_shared<robot_dart::Robot>("iiwa/iiwa.urdf", packages);
1818

1919
global_robot->fix_to_world();
2020
global_robot->set_position_enforced(true);

‎src/python/robot.cpp

+61-3
Original file line numberDiff line numberDiff line change
@@ -14,9 +14,21 @@ namespace robot_dart {
1414
using namespace robot_dart;
1515
// Robot class
1616
py::class_<Robot, std::shared_ptr<Robot>>(m, "Robot")
17-
.def(py::init<const std::string&, const std::vector<std::pair<std::string, std::string>>&, const std::string&, bool>())
18-
.def(py::init<const std::string&, const std::string&, bool>())
19-
.def(py::init<dart::dynamics::SkeletonPtr, const std::string&>())
17+
.def(py::init<const std::string&, const std::vector<std::pair<std::string, std::string>>&, const std::string&, bool, bool>(),
18+
py::arg("model_file"),
19+
py::arg("packages"),
20+
py::arg("robot_name") = "robot",
21+
py::arg("is_urdf_string") = false,
22+
py::arg("cast_shadows") = true)
23+
.def(py::init<const std::string&, const std::string&, bool, bool>(),
24+
py::arg("model_file"),
25+
py::arg("robot_name") = "robot",
26+
py::arg("is_urdf_string") = false,
27+
py::arg("cast_shadows") = true)
28+
.def(py::init<dart::dynamics::SkeletonPtr, const std::string&, bool>(),
29+
py::arg("skeleton"),
30+
py::arg("robot_name") = "robot",
31+
py::arg("cast_shadows") = true)
2032

2133
.def("clone", &Robot::clone)
2234
.def("clone_ghost", &Robot::clone_ghost,
@@ -48,6 +60,8 @@ namespace robot_dart {
4860
.def("fixed", &Robot::fixed)
4961
.def("free", &Robot::free)
5062

63+
.def("reset", &Robot::reset)
64+
5165
.def("set_actuator_types", &Robot::set_actuator_types,
5266
py::arg("type"),
5367
py::arg("joint_names") = std::vector<std::string>(),
@@ -111,24 +125,68 @@ namespace robot_dart {
111125
py::arg("positions"),
112126
py::arg("dof_names") = std::vector<std::string>())
113127

128+
.def("position_lower_limits", &Robot::position_lower_limits,
129+
py::arg("dof_names") = std::vector<std::string>())
130+
.def("set_position_lower_limits", &Robot::set_position_lower_limits,
131+
py::arg("positions"),
132+
py::arg("dof_names") = std::vector<std::string>())
133+
.def("position_upper_limits", &Robot::position_upper_limits,
134+
py::arg("dof_names") = std::vector<std::string>())
135+
.def("set_position_upper_limits", &Robot::set_position_upper_limits,
136+
py::arg("positions"),
137+
py::arg("dof_names") = std::vector<std::string>())
138+
114139
.def("velocities", &Robot::velocities,
115140
py::arg("dof_names") = std::vector<std::string>())
116141
.def("set_velocities", &Robot::set_velocities,
117142
py::arg("velocities"),
118143
py::arg("dof_names") = std::vector<std::string>())
119144

145+
.def("velocity_lower_limits", &Robot::velocity_lower_limits,
146+
py::arg("dof_names") = std::vector<std::string>())
147+
.def("set_velocity_lower_limits", &Robot::set_velocity_lower_limits,
148+
py::arg("velocities"),
149+
py::arg("dof_names") = std::vector<std::string>())
150+
.def("velocity_upper_limits", &Robot::velocity_upper_limits,
151+
py::arg("dof_names") = std::vector<std::string>())
152+
.def("set_velocity_upper_limits", &Robot::set_velocity_upper_limits,
153+
py::arg("velocities"),
154+
py::arg("dof_names") = std::vector<std::string>())
155+
120156
.def("accelerations", &Robot::accelerations,
121157
py::arg("dof_names") = std::vector<std::string>())
122158
.def("set_accelerations", &Robot::set_accelerations,
123159
py::arg("accelerations"),
124160
py::arg("dof_names") = std::vector<std::string>())
125161

162+
.def("acceleration_lower_limits", &Robot::acceleration_lower_limits,
163+
py::arg("dof_names") = std::vector<std::string>())
164+
.def("set_acceleration_lower_limits", &Robot::set_acceleration_lower_limits,
165+
py::arg("accelerations"),
166+
py::arg("dof_names") = std::vector<std::string>())
167+
.def("acceleration_upper_limits", &Robot::acceleration_upper_limits,
168+
py::arg("dof_names") = std::vector<std::string>())
169+
.def("set_acceleration_upper_limits", &Robot::set_acceleration_upper_limits,
170+
py::arg("accelerations"),
171+
py::arg("dof_names") = std::vector<std::string>())
172+
126173
.def("forces", &Robot::forces,
127174
py::arg("dof_names") = std::vector<std::string>())
128175
.def("set_forces", &Robot::set_forces,
129176
py::arg("forces"),
130177
py::arg("dof_names") = std::vector<std::string>())
131178

179+
.def("force_lower_limits", &Robot::force_lower_limits,
180+
py::arg("dof_names") = std::vector<std::string>())
181+
.def("set_force_lower_limits", &Robot::set_force_lower_limits,
182+
py::arg("forces"),
183+
py::arg("dof_names") = std::vector<std::string>())
184+
.def("force_upper_limits", &Robot::force_upper_limits,
185+
py::arg("dof_names") = std::vector<std::string>())
186+
.def("set_force_upper_limits", &Robot::set_force_upper_limits,
187+
py::arg("forces"),
188+
py::arg("dof_names") = std::vector<std::string>())
189+
132190
.def("commands", &Robot::commands,
133191
py::arg("dof_names") = std::vector<std::string>())
134192
.def("set_commands", &Robot::set_commands,

‎src/robot_dart/robot.cpp

+189-5
Original file line numberDiff line numberDiff line change
@@ -47,6 +47,22 @@ namespace robot_dart {
4747
return skeleton->getForces();
4848
else if (content == 4)
4949
return skeleton->getCommands();
50+
else if (content == 5)
51+
return skeleton->getPositionLowerLimits();
52+
else if (content == 6)
53+
return skeleton->getPositionUpperLimits();
54+
else if (content == 7)
55+
return skeleton->getVelocityLowerLimits();
56+
else if (content == 8)
57+
return skeleton->getVelocityUpperLimits();
58+
else if (content == 9)
59+
return skeleton->getAccelerationLowerLimits();
60+
else if (content == 10)
61+
return skeleton->getAccelerationUpperLimits();
62+
else if (content == 11)
63+
return skeleton->getForceLowerLimits();
64+
else if (content == 12)
65+
return skeleton->getForceUpperLimits();
5066
ROBOT_DART_EXCEPTION_ASSERT(false, "Unknown type of data!");
5167
}
5268

@@ -66,6 +82,22 @@ namespace robot_dart {
6682
data(i) = dof->getForce();
6783
else if (content == 4)
6884
data(i) = dof->getCommand();
85+
else if (content == 5)
86+
data(i) = dof->getPositionLowerLimit();
87+
else if (content == 6)
88+
data(i) = dof->getPositionUpperLimit();
89+
else if (content == 7)
90+
data(i) = dof->getVelocityLowerLimit();
91+
else if (content == 8)
92+
data(i) = dof->getVelocityUpperLimit();
93+
else if (content == 9)
94+
data(i) = dof->getAccelerationLowerLimit();
95+
else if (content == 10)
96+
data(i) = dof->getAccelerationUpperLimit();
97+
else if (content == 11)
98+
data(i) = dof->getForceLowerLimit();
99+
else if (content == 12)
100+
data(i) = dof->getForceUpperLimit();
69101
else
70102
ROBOT_DART_EXCEPTION_ASSERT(false, "Unknown type of data!");
71103
}
@@ -89,6 +121,22 @@ namespace robot_dart {
89121
return skeleton->setForces(data);
90122
else if (content == 4)
91123
return skeleton->setCommands(data);
124+
else if (content == 5)
125+
return skeleton->setPositionLowerLimits(data);
126+
else if (content == 6)
127+
return skeleton->setPositionUpperLimits(data);
128+
else if (content == 7)
129+
return skeleton->setVelocityLowerLimits(data);
130+
else if (content == 8)
131+
return skeleton->setVelocityUpperLimits(data);
132+
else if (content == 9)
133+
return skeleton->setAccelerationLowerLimits(data);
134+
else if (content == 10)
135+
return skeleton->setAccelerationUpperLimits(data);
136+
else if (content == 11)
137+
return skeleton->setForceLowerLimits(data);
138+
else if (content == 12)
139+
return skeleton->setForceUpperLimits(data);
92140
ROBOT_DART_EXCEPTION_ASSERT(false, "Unknown type of data!");
93141
}
94142

@@ -109,6 +157,22 @@ namespace robot_dart {
109157
dof->setForce(data(i));
110158
else if (content == 4)
111159
dof->setCommand(data(i));
160+
else if (content == 5)
161+
dof->setPositionLowerLimit(data(i));
162+
else if (content == 6)
163+
dof->setPositionUpperLimit(data(i));
164+
else if (content == 7)
165+
dof->setVelocityLowerLimit(data(i));
166+
else if (content == 8)
167+
dof->setVelocityUpperLimit(data(i));
168+
else if (content == 9)
169+
dof->setAccelerationLowerLimit(data(i));
170+
else if (content == 10)
171+
dof->setAccelerationUpperLimit(data(i));
172+
else if (content == 11)
173+
dof->setForceLowerLimit(data(i));
174+
else if (content == 12)
175+
dof->setForceUpperLimit(data(i));
112176
else
113177
ROBOT_DART_EXCEPTION_ASSERT(false, "Unknown type of data!");
114178
}
@@ -131,6 +195,22 @@ namespace robot_dart {
131195
return skeleton->setForces(skeleton->getForces() + data);
132196
else if (content == 4)
133197
return skeleton->setCommands(skeleton->getCommands() + data);
198+
else if (content == 5)
199+
return skeleton->setPositionLowerLimits(skeleton->getPositionLowerLimits() + data);
200+
else if (content == 6)
201+
return skeleton->setPositionUpperLimits(skeleton->getPositionUpperLimits() + data);
202+
else if (content == 7)
203+
return skeleton->setVelocityLowerLimits(skeleton->getVelocityLowerLimits() + data);
204+
else if (content == 8)
205+
return skeleton->setVelocityUpperLimits(skeleton->getVelocityUpperLimits() + data);
206+
else if (content == 9)
207+
return skeleton->setAccelerationLowerLimits(skeleton->getAccelerationLowerLimits() + data);
208+
else if (content == 10)
209+
return skeleton->setAccelerationUpperLimits(skeleton->getAccelerationUpperLimits() + data);
210+
else if (content == 11)
211+
return skeleton->setForceLowerLimits(skeleton->getForceLowerLimits() + data);
212+
else if (content == 12)
213+
return skeleton->setForceUpperLimits(skeleton->getForceUpperLimits() + data);
134214
ROBOT_DART_EXCEPTION_ASSERT(false, "Unknown type of data!");
135215
}
136216

@@ -151,6 +231,22 @@ namespace robot_dart {
151231
dof->setForce(dof->getForce() + data(i));
152232
else if (content == 4)
153233
dof->setCommand(dof->getCommand() + data(i));
234+
else if (content == 5)
235+
dof->setPositionLowerLimit(dof->getPositionLowerLimit() + data(i));
236+
else if (content == 6)
237+
dof->setPositionUpperLimit(dof->getPositionUpperLimit() + data(i));
238+
else if (content == 7)
239+
dof->setVelocityLowerLimit(dof->getVelocityLowerLimit() + data(i));
240+
else if (content == 8)
241+
dof->setVelocityUpperLimit(dof->getVelocityUpperLimit() + data(i));
242+
else if (content == 9)
243+
dof->setAccelerationLowerLimit(dof->getAccelerationLowerLimit() + data(i));
244+
else if (content == 10)
245+
dof->setAccelerationUpperLimit(dof->getAccelerationUpperLimit() + data(i));
246+
else if (content == 11)
247+
dof->setForceLowerLimit(dof->getForceLowerLimit() + data(i));
248+
else if (content == 12)
249+
dof->setForceUpperLimit(dof->getForceUpperLimit() + data(i));
154250
else
155251
ROBOT_DART_EXCEPTION_ASSERT(false, "Unknown type of data!");
156252
}
@@ -370,6 +466,14 @@ namespace robot_dart {
370466
return parent_jt->getType() == dart::dynamics::FreeJoint::getStaticType();
371467
}
372468

469+
void Robot::reset()
470+
{
471+
_skeleton->resetPositions();
472+
_skeleton->resetVelocities();
473+
_skeleton->resetAccelerations();
474+
this->clear_external_forces();
475+
}
476+
373477
void Robot::set_actuator_types(const std::string& type, const std::vector<std::string>& joint_names, bool override_mimic, bool override_base)
374478
{
375479
// Set all dofs to same actuator type
@@ -662,7 +766,7 @@ namespace robot_dart {
662766
return _skeleton->getCOMSpatialAcceleration();
663767
}
664768

665-
Eigen::VectorXd Robot::positions(const std::vector<std::string>& dof_names)
769+
Eigen::VectorXd Robot::positions(const std::vector<std::string>& dof_names) const
666770
{
667771
return detail::dof_data<0>(_skeleton, dof_names, _dof_map);
668772
}
@@ -672,7 +776,27 @@ namespace robot_dart {
672776
detail::set_dof_data<0>(positions, _skeleton, dof_names, _dof_map);
673777
}
674778

675-
Eigen::VectorXd Robot::velocities(const std::vector<std::string>& dof_names)
779+
Eigen::VectorXd Robot::position_lower_limits(const std::vector<std::string>& dof_names) const
780+
{
781+
return detail::dof_data<5>(_skeleton, dof_names, _dof_map);
782+
}
783+
784+
void Robot::set_position_lower_limits(const Eigen::VectorXd& positions, const std::vector<std::string>& dof_names)
785+
{
786+
detail::set_dof_data<5>(positions, _skeleton, dof_names, _dof_map);
787+
}
788+
789+
Eigen::VectorXd Robot::position_upper_limits(const std::vector<std::string>& dof_names) const
790+
{
791+
return detail::dof_data<6>(_skeleton, dof_names, _dof_map);
792+
}
793+
794+
void Robot::set_position_upper_limits(const Eigen::VectorXd& positions, const std::vector<std::string>& dof_names)
795+
{
796+
detail::set_dof_data<6>(positions, _skeleton, dof_names, _dof_map);
797+
}
798+
799+
Eigen::VectorXd Robot::velocities(const std::vector<std::string>& dof_names) const
676800
{
677801
return detail::dof_data<1>(_skeleton, dof_names, _dof_map);
678802
}
@@ -682,7 +806,27 @@ namespace robot_dart {
682806
detail::set_dof_data<1>(velocities, _skeleton, dof_names, _dof_map);
683807
}
684808

685-
Eigen::VectorXd Robot::accelerations(const std::vector<std::string>& dof_names)
809+
Eigen::VectorXd Robot::velocity_lower_limits(const std::vector<std::string>& dof_names) const
810+
{
811+
return detail::dof_data<7>(_skeleton, dof_names, _dof_map);
812+
}
813+
814+
void Robot::set_velocity_lower_limits(const Eigen::VectorXd& velocities, const std::vector<std::string>& dof_names)
815+
{
816+
detail::set_dof_data<7>(velocities, _skeleton, dof_names, _dof_map);
817+
}
818+
819+
Eigen::VectorXd Robot::velocity_upper_limits(const std::vector<std::string>& dof_names) const
820+
{
821+
return detail::dof_data<8>(_skeleton, dof_names, _dof_map);
822+
}
823+
824+
void Robot::set_velocity_upper_limits(const Eigen::VectorXd& velocities, const std::vector<std::string>& dof_names)
825+
{
826+
detail::set_dof_data<8>(velocities, _skeleton, dof_names, _dof_map);
827+
}
828+
829+
Eigen::VectorXd Robot::accelerations(const std::vector<std::string>& dof_names) const
686830
{
687831
return detail::dof_data<2>(_skeleton, dof_names, _dof_map);
688832
}
@@ -692,7 +836,27 @@ namespace robot_dart {
692836
detail::set_dof_data<2>(accelerations, _skeleton, dof_names, _dof_map);
693837
}
694838

695-
Eigen::VectorXd Robot::forces(const std::vector<std::string>& dof_names)
839+
Eigen::VectorXd Robot::acceleration_lower_limits(const std::vector<std::string>& dof_names) const
840+
{
841+
return detail::dof_data<9>(_skeleton, dof_names, _dof_map);
842+
}
843+
844+
void Robot::set_acceleration_lower_limits(const Eigen::VectorXd& accelerations, const std::vector<std::string>& dof_names)
845+
{
846+
detail::set_dof_data<9>(accelerations, _skeleton, dof_names, _dof_map);
847+
}
848+
849+
Eigen::VectorXd Robot::acceleration_upper_limits(const std::vector<std::string>& dof_names) const
850+
{
851+
return detail::dof_data<10>(_skeleton, dof_names, _dof_map);
852+
}
853+
854+
void Robot::set_acceleration_upper_limits(const Eigen::VectorXd& accelerations, const std::vector<std::string>& dof_names)
855+
{
856+
detail::set_dof_data<10>(accelerations, _skeleton, dof_names, _dof_map);
857+
}
858+
859+
Eigen::VectorXd Robot::forces(const std::vector<std::string>& dof_names) const
696860
{
697861
return detail::dof_data<3>(_skeleton, dof_names, _dof_map);
698862
}
@@ -702,7 +866,27 @@ namespace robot_dart {
702866
detail::set_dof_data<3>(forces, _skeleton, dof_names, _dof_map);
703867
}
704868

705-
Eigen::VectorXd Robot::commands(const std::vector<std::string>& dof_names)
869+
Eigen::VectorXd Robot::force_lower_limits(const std::vector<std::string>& dof_names) const
870+
{
871+
return detail::dof_data<11>(_skeleton, dof_names, _dof_map);
872+
}
873+
874+
void Robot::set_force_lower_limits(const Eigen::VectorXd& forces, const std::vector<std::string>& dof_names)
875+
{
876+
detail::set_dof_data<11>(forces, _skeleton, dof_names, _dof_map);
877+
}
878+
879+
Eigen::VectorXd Robot::force_upper_limits(const std::vector<std::string>& dof_names) const
880+
{
881+
return detail::dof_data<12>(_skeleton, dof_names, _dof_map);
882+
}
883+
884+
void Robot::set_force_upper_limits(const Eigen::VectorXd& forces, const std::vector<std::string>& dof_names)
885+
{
886+
detail::set_dof_data<12>(forces, _skeleton, dof_names, _dof_map);
887+
}
888+
889+
Eigen::VectorXd Robot::commands(const std::vector<std::string>& dof_names) const
706890
{
707891
return detail::dof_data<4>(_skeleton, dof_names, _dof_map);
708892
}

0 commit comments

Comments
 (0)