4am-robotics / cob_control Goto Github PK

• remove priority template: <typename PRIO = uint32_t> @ipa-fxm-cm
• separate and clearly name constraint builder/factory from actual constraints
• use *.cpp and create library instead of *_impl.h
• extract abstract GPM solver (i.e. just GPM formular) from actual GPM implementation (i.e. sum over constraint list)
• allow to use constraints with/without prediction, toggleable via parameter (could help with #64)

• optional: remove dynamic reconfigurability for solver types @ipa-fxm-cm

@fmessmer FYI

run roslint and cppcheck on (few more) packages

Can you drop a few words about how to use this tool?

After catkin_make install (install space is used to have all headers in one place):
cppcheck -I install/include/ -I /opt/ros/indigo/include/ --enable=all --quiet --force src/
src/ would match the full source space and can be limited to subdirectories, e.g. to check per package.
-I /usr/include/ could be added as well, but I am not sure if this is really helpful.

ToDo:

• investigate reset of PositionInterface (there are large jerks after no twist has been received for a while, i.e. vel vectors for simpson need to be cleared and then filled afresh!)
• add/test/optimize TrajectoryInterface (see fmessmer@a740835); reset needs to be considered here, too! 👉 related to #66
• add kinematic extension for Torso 👉 needs more testing
• add kinematic extension for Lookat 👉 done in #63
• investigate synergies between cob_obstacle_distance and ipa320/cob_manipulation#54 👉 moved to new separate issue #64

Compilation fails because newer urdf uses c++11 shared_ptr for urdf::JointConstSharedPtr

/var/tmp/portage/ros-kinetic/cob_omni_drive_controller-0.7.1/work/cob_omni_drive_controller-0.7.1/src/param_parser.cpp: In function ‘bool parseWheelGeom(WheelGeom&, XmlRpc::XmlRpcValue&, MergedXmlRpcStruct&, urdf::Model*)’:
/var/tmp/portage/ros-kinetic/cob_omni_drive_controller-0.7.1/work/cob_omni_drive_controller-0.7.1/src/param_parser.cpp:113:54: error: no match for ‘operator=’ (operand types are ‘boost::shared_ptr<const urdf::Joint>’ and ‘urdf::JointConstSharedPtr {aka std::shared_ptr<const urdf::Joint>}’)
         steer_joint = model->getJoint(geom.steer_name);
                                                      ^
In file included from /usr/include/boost/shared_ptr.hpp:17:0,
                 from /opt/ros/kinetic/include/ros/forwards.h:37,
                 from /opt/ros/kinetic/include/ros/common.h:37,
                 from /opt/ros/kinetic/include/ros/ros.h:43,
                 from /var/tmp/portage/ros-kinetic/cob_omni_drive_controller-0.7.1/work/cob_omni_drive_controller-0.7.1/include/cob_omni_drive_controller/UndercarriageCtrlGeomROS.h:21,
                 from /var/tmp/portage/ros-kinetic/cob_omni_drive_controller-0.7.1/work/cob_omni_drive_controller-0.7.1/src/param_parser.cpp:18:

@fmessmer FYI

I happened to come across this issue today when test driving paul-unity around with the joystick:

I saw the back_rotation joint kept moving slowly although I was not commanding anymore (I think mainly because the driver also went into QuickStop state, but I don't have proof for this). Interesting thing is the following plot where you can see, that there is still a non-zero wheel-command for the respective back_rotation joint!!!

I could also see the spikes on the velocity joint_states for various base joints:

For the rest of the time velocity joint_states is following the wheel_commands/target_velocity quite well:

related to https://github.com/mojin-robotics/cob4/issues/684

@fmessmer FYI

Hello, I am attempting to use cob_trajectory_controller with Schunk PowerCube in ROS Indigo. Currently cob_trajectory_controller will receive a trajectory from MoveIt! and calculate spline-points, I have not gotten the controller to go past this point.

Am I the first to run into this issue?

The error:
`[ INFO] [1532625716.016360995]: getting JointNames from parameter server: //arm_controller

[ INFO] [1532625716.019458137]: starting controller with DOF: 7 PTPvel: 0.400000 PTPAcc: 0.200000 maxError 0.150000

[ INFO] [1532625716.029240961]: Setting controller frequency to 100.000000 HZ

[ INFO] [1532625805.241194266]: Received new goal trajectory with 25 points

[ INFO] [1532625805.255916046]: Calculated 26 zwischenPunkte

[ INFO] [1532625805.257592111]: Calculated 26 splinepoints

terminate called after throwing an instance of 'std::out_of_range'
what(): vector::_M_range_check
[arm_controller/joint_trajectory_controller-1] process has died [pid 18717, exit code -6, cmd /home/ralab/Documents/ros/devel/lib/cob_trajectory_controller/cob_trajectory_controller __name:=joint_trajectory_controller __log:=/home/ralab/.ros/log/5717b96e-90f8-11e8-8fff-7085c274b189/arm_controller-joint_trajectory_controller-1.log].
log file: /home/ralab/.ros/log/5717b96e-90f8-11e8-8fff-7085c274b189/arm_controller-joint_trajectory_controller-1*.log
`

Best,

Kyle

I (still) think this is worth creating an issue: @ipa-bnm @ipa-fmw

When investigating the base command pipeline (twist_mux -> velocity_smoother -> control_plugin -> odometry_plugin), I found the following behavior for the "new" velocity smoother.

Tested with a cpp-node that publishes a sine signal onto a geometry_msgs::Twist topic (see #93)
roslaunch cob_bringup velocity_smoother.launch robot:=cob4-1

100Hz

50 Hz

30Hz

Maybe it's just a matter of improving the configuration....

@fmessmer FYI

I'm separating this into a new issue from #77

MoveIt!-based approach is proposed in ipa320/cob_manipulation#64 👉 merged!

ToDo:

• separate message/service definitions into "neutral" location (e.g. cob_common) 👉 done in #100 ipa320/cob_manipulation#66
• use "neutral" message/service definitions within cob_obstacle_distance, cob_obstacle_distance_moveit and cob_twist_controller 👉 done in #100 ipa320/cob_manipulation#66
• use cob_obstacle_distance_moveit together with cob_twist_controller
  (replacing cob_obstacle_distance)
  • adjust service and topoic names in both cob_twist_controller and cob_obstacle_distance_moveit (i.e. use remap in the launch file provided in ipa320/cob_robots#460)
  • remove component-specific instance of cob_obstacle_distance from generic_cartesian_controller.launch file, i.e. this line
• deprecation of cob_obstacle_distance package (optional)

@fmessmer FYI

If an active follow_joint_trajectory action is aborted, the cob_trajectory_controller will never stop the control chain. Indeed he is sending the last set velocity to the driver in a loop.

This error happens if, for example, the moveit trajectory execution monitor stops a trajectory.

especially the various waitForTransform/lookupTransform structures....:point_right: CONSISTENCY!!!

Verify how much time those combos take, e.g.: https://github.com/ipa320/cob_control/pull/74/files#r47382030
and try to optimize

Also, take care of this comment

Try to use common update_rate throughout all framework (i.e. cob_cartesian_controller, cob_frame_tracker, cob_twist_controller)

@fmessmer FYI

further investigate behavior of twist_controller when using it with JointTrajectoryControllerInterface

in #66 it has been tested/verified that both the behavior of the utils MovingAverage as well as SimpsonIntegration are fine and perform acceptably
still during tests with hardware - torso in particular - resulting motion were quite jerky.

this can be seen in quite jerky joint_trajectory_controller/state/desired messages (velocities as well as positions).
assumption is that the joint_trajectory_controller is not meant to be commanded at high update_rates (50Hz) - it's much less jerky when commanding at only 10Hz
as the jerky motion has mainly be observed on Torso (Elmo), there also might be potential in firmware configuration and/or motor/sensor hardware optimization @ipa-mdl @ipa-tif @ipa-bdw

There are quite some nodes using python2 print

./cob_twist_controller/scripts/test/test_raw31.py
./cob_twist_controller/scripts/test/test_careobot_base.py
./cob_twist_controller/scripts/test_publisher_twist_series.py
./cob_twist_controller/scripts/test_publisher_twist_sine.py
./cob_twist_controller/scripts/evaluate_dbg_jnt_velocity_tests.py
./cob_twist_controller/scripts/test_publisher_vel.py
./cob_twist_controller/src/data_collection/data_collection.py
./cob_obstacle_distance/scripts/test_interactive_obstacle_node.py
./cob_frame_tracker/scripts/interactive_frame_target.py
./cob_cartesian_controller/scripts/test_move_lin.py
./cob_cartesian_controller/scripts/test_move_circ.py
./cob_cartesian_controller/src/simple_cartesian_interface/simple_cartesian_interface.py
./cob_model_identifier/scripts/input_series_twist.py

use proper rospy.logXXX (or at least python3-compatible print())

Also get rid of std::cout:

./cob_undercarriage_ctrl_node/src/cob_undercarriage_ctrl_new.cpp
./cob_trajectory_controller/common/include/cob_trajectory_controller/RefValJS_PTP.h
./cob_model_identifier/src/output_recorder.cpp
./cob_collision_velocity_filter/src/cob_collision_velocity_filter.cpp

@fmessmer FYI

discussion related to #32

This node should get the convex polygon footprint from the footprint observer (see #131) and disable the respective velocity outputs in the twist_mux, once the distance to an obstacle gets below a certain safety distance.

@fmessmer FYI

The following open issues have been separated from #52

• add toggle/boolean parameter/variable that allows enabling/disabling the abortian criteria check (both to ActionGoal, ServiceRequest and DynamicReconfigure)
• better Action interface for cob_cartesian_controller (e.g. separate MoveLin and MoveCirc actions?)
• finalize/unify Python interface (simple_cartesian_interface)
• use Action interface of cob_frame_tracker rather than Service interface
• rework/improve safety-/abortion-/hold_twist criteria (remove from cob_cartesian_controller, improve in cob_frame_tracker)
• in the end start_tracking service should completely be replaced by respective action (?)
• also provide an action for start_lookat...maybe from within the start_tracking action with a respective boolean flag for toggling between tracking and lookat

@fmessmer FYI

• We need acceleration limiters: see https://github.com/ipa320/cob_control/issues/53#issuecomment-142904976
• We need a consistent way for configuring limits (pos(-tolerances), vel_limits, acc_limits)!
  They could be retrieved from URDF, joint_limits.yaml and reconfigure and then always choose the "tightest" limits

Desperately waiting for it :)
Is there any blocking issue or anything I can help you with?

xref: ipa320/schunk_modular_robotics#142

I successfully tested the new ur-modern_driver which allow us to send speed commands to the UR arm.

However when I was trying to use it with twist controller I got the following:

[ERROR] [1492072763.263901255]: Client [/arm/ur_driver] wants topic /arm/joint_group_velocity_controller/command to have datatype/md5sum [trajectory_msgs/JointTrajectory/65b4f94a94d1ed67169da35a02f33d3f], but our version has [std_msgs/Float64MultiArray/4b7d974086d4060e7db4613a7e6c3ba4]. Dropping connection.

What do you recommend to do? Should we change the type in twist controller of the publish topic?

My robot is Schunk_lwa4d, I can move it use the dashboard interface.
Ｔｈｅｎ I roslaunch [cartesian_controller.launch] and rosrun [move_lin.py](in schunk_lwa4d package),
the line [sss.move("arm"...] can move the arm, but the line [sci.move_lin(pose, "arm_base_link", profile)] failed, the terminal showed as follow:

1. [WARN] [WallTime: 1516411841.868259] Waiting for ActionServer: /cartesian_trajectory_action
2. [ERROR] [WallTime: 1516411845.868698] ActionServer not available within timeout

I don't know how to solve this.
I hope you can help me.
Thanks in advance.

This is a detailed summary of the investigations performed last week - on cob4-8 hardware:
(For previous investigations in simulation see here)

Main objectives:

• investigate behaviour of drive and steer joints individually
• investigate behaviour of joints when commanded by omni_drive_controller
• in order to identify potential for optimization of reactivity of controller
• in order to identify potential for optimization of accuracy of controller

Current controller implementation/configuration results in e.g. the following step response showing

• input Cartesian twist + resulting odometry (center)
• drive joint target velocities + drive joint velocities (left)
• steer joint target velocities + steer joint target error (right)

Note that currently both drive and steer joints are commanded in VelocityMode using the Mass-Spring-Damper-Control algorithm with fixed parameters (from yaml).

The reaction of the steer joints is of particular interest:

It can be seen that the steer joints require about ~1.0 sec for reaching the target steer position (i.e. target veloctiy == 0.0 again) with a significant velocity ramp on initial stimulus and overshoot/oscilation (steer_target_velocity[1]).

The initial ramp is due to cut-off limitation of steer drive acceleration.
There also is a joint-specific cut-off limitation of the steer drive velocity. This might lead to non-coordinated movement of the wheels. Which might lead to wear on the base frame.

With #91 and #140, experiments were performed in order to optimize controller behavior by modifying/tuning control parameters:
First the controller-internal acceleration/velocity limitation has been disabled. This leads to a quicker reaction after receiving an input stimulus:

Still there is significant overshoot and settle time is still about 1.0 sec.

Note there still is an acceleration/velocity limitation within the Elmo controller itself which can be seen in the following plot (graphs with dots depict joint_states). It can be seen that target velocities are un-limited (in left plot) while measured joint_states shows a limitation.

Experimental modifications (i.e. without proper system identification and control engineering expertise) of mass, spring and damping parameters did not lead to a significantly better behavior (oscillation/escalation of steer joints).

Another interesting effect of the velocity-controlled steer joints is depicted in the following plot:

It clearly shows a steer-position-dependend, 360°-periodic velocity noise (joint velocity command vs. joint velocity) in the magnitude of about +/-0.5 rad/sec.

As steer joints are not designed/proportioned/configured for velocity control, position mode for the steer joint has been tested as well. Position mode shows much better accuracy and reactivity as well as less noise in the sensor data.
This is well worth further investigation...

Available new features of ros_control (from Jade onwords, because API-changing) allow the implementation of multiple/mixed interfaces within a single controller. Thus a controller is proposed in #140 for Kinetic using position-controlled steer joints and velocity-controlled drive joints.
More on this in a separate issue #144

@ipa-mdl @ipa-fmw @ipa-mig @ipa-bnm @ipa-flg @ipa-tif @ipa-nhg @fmessmer FYI
Please leave comments/suggestions/questions 😉

Moving this issue from #53 into a separate issue

Discussion was stared in this comment.
So far, the possibility to use both damped inverse and undamped inverse during the GPM projection has been implemented (see https://github.com/ipa320/cob_control/issues/53#issuecomment-142896187). As first mathematical verifications (i.e. doing Forward kinematics) proved to be SelfMotions, indeed.

However, while discussing it with @ipa-fxm-mb as well as due to results from the octave scripts the argument war raised that the verification might only be correct for cases wher damping is low...i.e. far away from singular configuration.

Thus, this phenomenon needs to be investigated further in situations where GPM is used close to singular configurations.

• re-verify that self-motions are self-motions in case damping is significant - use constant damping (See discussion with @ipa-fxm-mb and respective octave scripts
• apply fixes to all solvers (i.e. GPM, StackOfTasks, TaskPriority)

The current implementation cannot be used when the kinematic chain is extended by selecting available KinematicExtensions (e.g. BaseActive, COB_TORSO or LOOKAT).

This is due to the fact that the constraint (at it's initialization) receives a KDL::Chain that only includes the (non-extended) main chain (e.g arm_left). This chain is used to initialize FKSolvers as well as JntToJacSolvers (for prediction) within CA.
However, this leads to dimension mismatches as soon as e.g. adjustedJacobian or adjustedJointStates are passed in due to active KinematicExtensions.

For now, whenever a KinematicExtension is activated, CA constraint is disabled!!!

In order to solve this a significant refactoring of the CA implementaiton is required

@fmessmer FYI

e.g. in GPM-solver check whether projector is Null-Matrix, which means pinv * this->jacobian_data_ is Eigen::MatrixXd::Identity which means this->jacobian_data_ is invertable/regular and thus Nullspace is empty thus the projection has not influence on q_dots_out

Add a check for this, print a warning and skip loop over constraints simply setting Eigen::MatrixXd qdots_out = particular_solution

This would help understand why e.g. collisions happen although CA is activated!

Do this for Stack-of-Task and Task-2nd-Prio, too

@fmessmer FYI

this is needed for MLR bases

@fmessmer FYI

When testing cob_bringup on cob4-8 under kinetic, I get the following warning messages once per second (copied from rqt_console):

Node: /collision_velocity_filter
Time: 14:38:41.047085207 (2017-02-17)
Severity: Warn
Published Topics: /base/twist_mux/command_safe, /collision_velocity_filter/anti_collision_costmap/costmap, /collision_velocity_filter/anti_collision_costmap/costmap_updates, /collision_velocity_filter/anti_collision_costmap/footprint, /collision_velocity_filter/anti_collision_costmap/obstacle_layer/clearing_endpoints, /collision_velocity_filter/anti_collision_costmap/obstacle_layer/parameter_descriptions, /collision_velocity_filter/anti_collision_costmap/obstacle_layer/parameter_updates, /collision_velocity_filter/anti_collision_costmap/parameter_descriptions, /collision_velocity_filter/anti_collision_costmap/parameter_updates, /collision_velocity_filter/parameter_descriptions, /collision_velocity_filter/parameter_updates, /collision_velocity_filter/velocity_limited_marker, /relevant_obstacles_grid, /rosout

Could not get robot pose, cancelling reconfiguration

Location:
/tmp/binarydeb/ros-kinetic-costmap-2d-1.14.0/src/costmap_2d_ros.cpp:Costmap2DROS::movementCB:355

Node: /collision_velocity_filter
Time: 14:38:40.959705538 (2017-02-17)
Severity: Warn
Published Topics: /base/twist_mux/command_safe, /collision_velocity_filter/anti_collision_costmap/costmap, /collision_velocity_filter/anti_collision_costmap/costmap_updates, /collision_velocity_filter/anti_collision_costmap/footprint, /collision_velocity_filter/anti_collision_costmap/obstacle_layer/clearing_endpoints, /collision_velocity_filter/anti_collision_costmap/obstacle_layer/parameter_descriptions, /collision_velocity_filter/anti_collision_costmap/obstacle_layer/parameter_updates, /collision_velocity_filter/anti_collision_costmap/parameter_descriptions, /collision_velocity_filter/anti_collision_costmap/parameter_updates, /collision_velocity_filter/parameter_descriptions, /collision_velocity_filter/parameter_updates, /collision_velocity_filter/velocity_limited_marker, /relevant_obstacles_grid, /rosout

Costmap2DROS transform timeout. Current time: 1487338720.9596, global_pose stamp: 0.0000, tolerance: 0.3000

Location:
/tmp/binarydeb/ros-kinetic-costmap-2d-1.14.0/src/costmap_2d_ros.cpp:Costmap2DROS::getRobotPose:538

@ipa-mig @ipa-fmw @ipa-nhg
I see the global_pose.stamp is 0.0, but I don't know why...
Do I need to localize or do something to set the robot_pose?
Is this a wrong configuration of cob4-8?
It's not just an issue of cob4-8!

Hints and help appreciated!

@fmessmer FYI

• controller that chains other controllers
• soft limit according to http://wiki.ros.org/pr2_controller_manager/safety_limits
• enforce hard position limits for all interfaces
• open loop maximum vel, acc, eff filters

@fmessmer FYI

problem mainly is that parameters are re-used, but require different default values/ranges for each use-case
this leads to conflicts/breaking previous behavior when default values get changed in cfg/data_types.h

-> refactor/make parameters solver-/damping-/constraint-specific (e.g. via namespace?)

Currently, the cob_footprint_observer only outputs a rectangular footprint. This should be adapted to provide a convex polygon. #32

One could think of two possible stages:

1. add dummy links to cover the full size of the robot
2. use the dimension of the meshes somehow

@fmessmer FYI

• Is the package still usable/compatible with latest cob_twist_controller
• Can it be used to improve the behavior of cob_twist_controller, i.e. better PID-values
• Simplify usage via scripts, automatic evaluation,...and document it!
• Extend the package in order to support additional models

@fmessmer FYI

Can we add some kind of an "upright"-bias that tries to keep components "upright"?
This would be really helpful for e.g. lookat, as sometimes "the torso is bent just to look at a goal that is just in front of cob", like so:

@fmessmer FYI

Would it be possible to release the cob_velocity_smoother for ROS melodic?

Example: http://build.ros.org/view/Ibin_arm_uThf/job/Ibin_arm_uThf__cob_cartesian_controller__ubuntu_trusty_armhf__binary/222/console

04:05:19 [100%] Building CXX object CMakeFiles/cartesian_controller_node.dir/src/cartesian_controller_node.cpp.o
04:05:19 /usr/bin/arm-linux-gnueabihf-g++   -DROSCONSOLE_BACKEND_LOG4CXX -DROS_PACKAGE_NAME=\"cob_cartesian_controller\" -g -O2 -fstack-protector --param=ssp-buffer-size=4 -Wformat -Werror=format-security -DNDEBUG -D_FORTIFY_SOURCE=2  -I/tmp/binarydeb/ros-indigo-cob-cartesian-controller-0.6.14/obj-arm-linux-gnueabihf/devel/include -I/tmp/binarydeb/ros-indigo-cob-cartesian-controller-0.6.14/include -I/opt/ros/indigo/include    -o CMakeFiles/cartesian_controller_node.dir/src/cartesian_controller_node.cpp.o -c /tmp/binarydeb/ros-indigo-cob-cartesian-controller-0.6.14/src/cartesian_controller_node.cpp
04:06:25 Linking CXX executable devel/lib/cob_cartesian_controller/cartesian_controller_node
04:06:25 /usr/bin/cmake -E cmake_link_script CMakeFiles/cartesian_controller_node.dir/link.txt --verbose=1
04:06:25 /usr/bin/arm-linux-gnueabihf-g++   -g -O2 -fstack-protector --param=ssp-buffer-size=4 -Wformat -Werror=format-security -DNDEBUG -D_FORTIFY_SOURCE=2     CMakeFiles/cartesian_controller_node.dir/src/cartesian_controller_node.cpp.o  -o devel/lib/cob_cartesian_controller/cartesian_controller_node -rdynamic devel/lib/libcartesian_controller.so /opt/ros/indigo/lib/libtf.so /opt/ros/indigo/lib/libtf2_ros.so /opt/ros/indigo/lib/libactionlib.so /opt/ros/indigo/lib/libmessage_filters.so /opt/ros/indigo/lib/libroscpp.so -lboost_signals -lboost_filesystem /opt/ros/indigo/lib/libxmlrpcpp.so /opt/ros/indigo/lib/libtf2.so /opt/ros/indigo/lib/librosconsole.so /opt/ros/indigo/lib/librosconsole_log4cxx.so /opt/ros/indigo/lib/librosconsole_backend_interface.so -llog4cxx -lboost_regex /opt/ros/indigo/lib/libroscpp_serialization.so /opt/ros/indigo/lib/librostime.so -lboost_date_time /opt/ros/indigo/lib/libcpp_common.so -lboost_system -lboost_thread -lpthread -lconsole_bridge devel/lib/libtrajectory_interpolator.so devel/lib/libprofile_generator.so devel/lib/libcartesian_controller_utils.so /opt/ros/indigo/lib/libtf.so /opt/ros/indigo/lib/libtf2_ros.so /opt/ros/indigo/lib/libactionlib.so /opt/ros/indigo/lib/libmessage_filters.so /opt/ros/indigo/lib/libroscpp.so -lboost_signals -lboost_filesystem /opt/ros/indigo/lib/libxmlrpcpp.so /opt/ros/indigo/lib/libtf2.so /opt/ros/indigo/lib/librosconsole.so /opt/ros/indigo/lib/librosconsole_log4cxx.so /opt/ros/indigo/lib/librosconsole_backend_interface.so -llog4cxx -lboost_regex /opt/ros/indigo/lib/libroscpp_serialization.so /opt/ros/indigo/lib/librostime.so -lboost_date_time /opt/ros/indigo/lib/libcpp_common.so -lboost_system -lboost_thread -lpthread -lconsole_bridge -Wl,-rpath,/tmp/binarydeb/ros-indigo-cob-cartesian-controller-0.6.14/obj-arm-linux-gnueabihf/devel/lib:/opt/ros/indigo/lib: 
04:06:26 devel/lib/libprofile_generator.so: undefined reference to `vtable for TrajectoryProfileRamp'
04:06:26 devel/lib/libprofile_generator.so: undefined reference to `vtable for TrajectoryProfileSinoid'
04:06:26 collect2: error: ld returned 1 exit status
04:06:26 make[4]: *** [devel/lib/cob_cartesian_controller/cartesian_controller_node] Error 1
04:06:26 make[4]: Leaving directory `/tmp/binarydeb/ros-indigo-cob-cartesian-controller-0.6.14/obj-arm-linux-gnueabihf'
04:06:26 make[3]: *** [CMakeFiles/cartesian_controller_node.dir/all] Error 2
04:06:26 make[3]: Leaving directory `/tmp/binarydeb/ros-indigo-cob-cartesian-controller-0.6.14/obj-arm-linux-gnueabihf'
04:06:26 make[2]: *** [all] Error 2
04:06:26 make[2]: Leaving directory `/tmp/binarydeb/ros-indigo-cob-cartesian-controller-0.6.14/obj-arm-linux-gnueabihf'
04:06:26 dh_auto_build: make -j1 returned exit code 2
04:06:26 make[1]: *** [override_dh_auto_build] Error 2
04:06:26 make[1]: Leaving directory `/tmp/binarydeb/ros-indigo-cob-cartesian-controller-0.6.14'
04:06:26 make: *** [build] Error 2
04:06:26 dpkg-buildpackage: error: debian/rules build gave error exit status 2
04:06:26 E: Building failed

Debug, verify and investigate:

• mathematical formula of moving_average DONE
• mathematical formula of simpson_integration
• reset condition for integration
• initiall value of positional command in comparison to actual current joint position (there appears to be a jump in the first command message)
• smoothness of positional command (values)
• equidistance-ness of positional command (timestamps)

ToDo's:

• review, verify and re-structure profile_generator library (unification of sinoid/ramp for linear/circular, use design pattern) #74
• better Action interface for cob_cartesian_controller (e.g. separate MoveLin and MoveCirc actions?) #75
• more Cartesian trajectories (e.g. 3-point MoveCirc) #69
• more Cartesian trajectories (e.g. list of waypoints) #69
• trajectory blending #69
• finalize/unify Python interface (simple_cartesian_interface) #75
• How can we guarantee that the end-effector is at the correct starting pose for a given Cartesian motion?
• use Action interface of cob_frame_tracker rather than Service interface #75
• rework/improve safety-/abortion-/hold_twist criteria (remove from cob_cartesian_controller, improve in cob_frame_tracker) #75
• limit twists (either outgoing provided cob_frame_tracker or incoming into cob_twist_controller
• finally roslint the package (http://wiki.ros.org/roslint) #74

together with @ipa-fxm-cm

After clone the package on the cob4-2, I couldn't launch the robot setting the install folder as ros package path. Delete the install folder and make it again, didn't works

Separating this from #52

Ideas for additional interfaces for commanding Cartesian paths:

• move_circ (using 3 points(/poses?) to define the circular plane)
• move_lin (using a list of waypoints, instead of just a single one - stopping at each point)
• blending (not stopping at each waypoint)
• move_csv (read Cartesian goal poses from a csv file)
• move_math (i.e. provide a mathematical function f : t --> tf_frame, e.g. Rose

Maybe also make use of techniques used in https://github.com/ipa320/cobvid

@ipa-fxm-cm @fmessmer FYI

separating this from #62

(Dynamic reconfigure is interesting for the controller settings..) (@ipa-mdl)

Motivated by the insights documented in #143 this issue is to document further tests/developments regarding wheel-multi/mixed-mode cob_omni_drive_controller in order to be able to control the steer motors in position mode

Next steps:

• More detailed tests of #140 on cob-base (Kinetic Setup)
• optimize position control approach of wheel-multi/mixed-mode controller

It might even be desirable to involve/implement CPC's work...

@ipa-mdl @ipa-fmw @ipa-mig @ipa-bnm @ipa-flg @ipa-tif @ipa-nhg @fmessmer FYI
Please leave comments/suggestions/questions 😉

hello.
I want to use cob_cartesian_controller.
so I installed controller by apt-get install ros-kinetic-cob-cartesian-controller
I run the test code "rosrun cob_cartesian_controller test_move_circ.py"
but only I can see
[WARN] [1542285632.906014, 393.315000]: Waiting for ActionServer: /cartesian_trajectory_action

so what is the problem?

and I lauched "roslaunch cob_bringup generic_cartesian_controller.launch
but only error like
[/opt/ros/kinetic/share/cob_bringup/controllers/generic_cartesian_controller.launch] requires the 'component_name' arg to be set
The traceback for the exception was written to the log file

I want to simulate cob manipulater by cartesian control first and run hardware.
OS is ubuntu 16.04 and ros kinetic.

thank you

• TF frames
• covariance

see http://jenkins.ros.org/job/devel-indigo-cob_control/ARCH_PARAM=amd64,UBUNTU_PARAM=trusty,label=devel/30/console

Maybe cob_obstacle distance might be replaced by using a more MoveIt-oriented approach as in cob_collision_monitor package....investigate synergies between cob_obstacle_distance and ipa320/cob_manipulation#54
Also, have a look at https://github.com/koellsch/obstacle_distance

<remap from="command_in" to="/base/collision_velocity_filter/command" />
<remap from="command" to="/base/twist_mux/command_safe" />

are not working.

Needs to be investigated further, but here are some details I figured out so far:
On schunk_lwa4d with current JointState (see below), sending Zero-Twist results in a constant velocity command (see rqt_plot), TwistController conifg (see below):

fxm@sony-vaio:~$ rostopic echo /arm/joint_states -n 1
header: 
  seq: 71361
  stamp: 
    secs: 1443087826
    nsecs: 351550793
  frame_id: ''
name: ['arm_1_joint', 'arm_2_joint', 'arm_3_joint', 'arm_4_joint', 'arm_5_joint', 'arm_6_joint', 'arm_7_joint']
position: [-0.6311459641061895, 1.0047162372030558, -0.021048670779051617, -1.6828988246504923, -1.7275268936239874, 0.7537902506438311, 1.8116168569850744]
velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
effort: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

---

joint_limits:

<property name="safety_offset" value="0.02"/>
<limit effort="216" velocity="0.43633" lower="${-M_PI+safety_offset}" upper="${M_PI-safety_offset}"/>
<limit effort="216" velocity="0.43633" lower="${-2.14 + safety_offset}" upper="${2.14 - safety_offset}"/>
<limit effort="81.5" velocity="0.4189" lower="${-M_PI + safety_offset}" upper="${M_PI - safety_offset}"/>
<limit effort="81.5" velocity="0.4189" lower="${-2.18 + safety_offset}" upper="${2.18 - safety_offset}"/>
<limit effort="20.7" velocity="0.43633" lower="${-M_PI + safety_offset}" upper="${M_PI - safety_offset}"/>
<limit effort="15" velocity="1.2566" lower="${-2.09 + safety_offset}" upper="${2.09 - safety_offset}"/>
<limit effort="15" velocity="1.2566" lower="${-2.96 + safety_offset}" upper="${2.96 - safety_offset}"/>

rviz screenshot:

rqt_plot screenshot:

From left to right, I swtched JLA_OFF, JLA_ON, JLA_MID, JLA_INEQ.

arm/twist_controller config:

fxm@sony-vaio:~$ rosparam get /arm
arm_1_joint_position_controller: {joint: arm_1_joint, required_drive_mode: 1, type: position_controllers/JointPositionController}
arm_1_joint_velocity_controller: {joint: arm_1_joint, required_drive_mode: 2, type: velocity_controllers/JointVelocityController}
arm_2_joint_position_controller: {joint: arm_2_joint, required_drive_mode: 1, type: position_controllers/JointPositionController}
arm_2_joint_velocity_controller: {joint: arm_2_joint, required_drive_mode: 2, type: velocity_controllers/JointVelocityController}
arm_3_joint_position_controller: {joint: arm_3_joint, required_drive_mode: 1, type: position_controllers/JointPositionController}
arm_3_joint_velocity_controller: {joint: arm_3_joint, required_drive_mode: 2, type: velocity_controllers/JointVelocityController}
arm_4_joint_position_controller: {joint: arm_4_joint, required_drive_mode: 1, type: position_controllers/JointPositionController}
arm_4_joint_velocity_controller: {joint: arm_4_joint, required_drive_mode: 2, type: velocity_controllers/JointVelocityController}
arm_5_joint_position_controller: {joint: arm_5_joint, required_drive_mode: 1, type: position_controllers/JointPositionController}
arm_5_joint_velocity_controller: {joint: arm_5_joint, required_drive_mode: 2, type: velocity_controllers/JointVelocityController}
arm_6_joint_position_controller: {joint: arm_6_joint, required_drive_mode: 1, type: position_controllers/JointPositionController}
arm_6_joint_velocity_controller: {joint: arm_6_joint, required_drive_mode: 2, type: velocity_controllers/JointVelocityController}
arm_7_joint_position_controller: {joint: arm_7_joint, required_drive_mode: 1, type: position_controllers/JointPositionController}
arm_7_joint_velocity_controller: {joint: arm_7_joint, required_drive_mode: 2, type: velocity_controllers/JointVelocityController}
chain_base_link: arm_podest_link
chain_tip_link: arm_7_link
driver:
  bus: {device: can0}
  defaults:
    dcf_overlay: {604Csub1: '1', 604Csub2: '24000'}
    eds_file: config/Schunk_0_63.dcf
    eds_pkg: schunk_lwa4d
    vel_to_device: rint(rad2deg(vel)*250)
  name: arm
  nodes:
    arm_1_joint: {id: 3}
    arm_2_joint: {id: 4}
    arm_3_joint: {id: 5}
    arm_4_joint: {id: 6}
    arm_5_joint: {id: 7}
    arm_6_joint: {id: 8}
    arm_7_joint: {id: 9}
  sync: {interval_ms: 10, overflow: 0}
frame_tracker:
  cart_min_dist_threshold_lin: 0.1
  cart_min_dist_threshold_rot: 0.1
  marker_scale: 0.3
  movable_rot: true
  movable_trans: true
  pid_rot_x: {d: 0.0, i: 0.0, i_clamp: 0.0, i_clamp_max: 0.0, i_clamp_min: -0.0, p: 1.0}
  pid_rot_y: {d: 0.0, i: 0.0, i_clamp: 0.0, i_clamp_max: 0.0, i_clamp_min: -0.0, p: 1.0}
  pid_rot_z: {d: 0.0, i: 0.0, i_clamp: 0.0, i_clamp_max: 0.0, i_clamp_min: -0.0, p: 1.0}
  pid_trans_x: {d: 0.0, i: 0.0, i_clamp: 0.0, i_clamp_max: 0.0, i_clamp_min: -0.0,
    p: 1.0}
  pid_trans_y: {d: 0.0, i: 0.0, i_clamp: 0.0, i_clamp_max: 0.0, i_clamp_min: -0.0,
    p: 1.0}
  pid_trans_z: {d: 0.0, i: 0.0, i_clamp: 0.0, i_clamp_max: 0.0, i_clamp_min: -0.0,
    p: 1.0}
  tracking_frame: arm_7_target
  twist_dead_threshold_lin: 0.05
  twist_dead_threshold_rot: 0.05
  twist_deviation_threshold_lin: 0.5
  twist_deviation_threshold_rot: 0.5
  update_rate: 50.0
joint_group_interpol_position_controller:
  joints: [arm_1_joint, arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint, arm_6_joint,
    arm_7_joint]
  required_drive_mode: 7
  type: position_controllers/JointGroupPositionController
joint_group_position_controller:
  joints: [arm_1_joint, arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint, arm_6_joint,
    arm_7_joint]
  required_drive_mode: 1
  type: position_controllers/JointGroupPositionController
joint_group_velocity_controller:
  joints: [arm_1_joint, arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint, arm_6_joint,
    arm_7_joint]
  required_drive_mode: 2
  type: velocity_controllers/JointGroupVelocityController
joint_limits:
  arm_1_joint: {has_acceleration_limits: true, has_effort_limits: true, has_jerk_limits: true,
    has_velocity_limits: true, max_acceleration: 0.4, max_effort: 5.0, max_jerk: 100.0,
    max_velocity: 0.4}
  arm_2_joint: {has_acceleration_limits: true, has_effort_limits: true, has_jerk_limits: true,
    has_velocity_limits: true, max_acceleration: 0.4, max_effort: 5.0, max_jerk: 100.0,
    max_velocity: 0.4}
  arm_3_joint: {has_acceleration_limits: true, has_effort_limits: true, has_jerk_limits: true,
    has_velocity_limits: true, max_acceleration: 0.4, max_effort: 5.0, max_jerk: 100.0,
    max_velocity: 0.4}
  arm_4_joint: {has_acceleration_limits: true, has_effort_limits: true, has_jerk_limits: true,
    has_velocity_limits: true, max_acceleration: 0.4, max_effort: 5.0, max_jerk: 100.0,
    max_velocity: 0.4}
  arm_5_joint: {has_acceleration_limits: true, has_effort_limits: true, has_jerk_limits: true,
    has_velocity_limits: true, max_acceleration: 0.4, max_effort: 5.0, max_jerk: 100.0,
    max_velocity: 0.4}
  arm_6_joint: {has_acceleration_limits: true, has_effort_limits: true, has_jerk_limits: true,
    has_velocity_limits: true, max_acceleration: 1.0, max_effort: 5.0, max_jerk: 100.0,
    max_velocity: 1.0}
  arm_7_joint: {has_acceleration_limits: true, has_effort_limits: true, has_jerk_limits: true,
    has_velocity_limits: true, max_acceleration: 1.0, max_effort: 5.0, max_jerk: 100.0,
    max_velocity: 1.0}
joint_names: [arm_1_joint, arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint, arm_6_joint,
  arm_7_joint]
joint_state_controller: {publish_rate: 50, type: joint_state_controller/JointStateController}
joint_trajectory_controller:
  action_monitor_rate: 10
  constraints:
    arm_1_joint: {goal: 0.1, trajectory: 0.1}
    arm_2_joint: {goal: 0.1, trajectory: 0.1}
    arm_3_joint: {goal: 0.1, trajectory: 0.1}
    arm_4_joint: {goal: 0.1, trajectory: 0.1}
    arm_5_joint: {goal: 0.1, trajectory: 0.1}
    arm_6_joint: {goal: 0.1, trajectory: 0.1}
    arm_7_joint: {goal: 0.1, trajectory: 0.1}
    goal_time: 0.6
    stopped_velocity_tolerance: 0.05
  joints: [arm_1_joint, arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint, arm_6_joint,
    arm_7_joint]
  required_drive_mode: 7
  state_publish_rate: 25
  stop_trajectory_duration: 0.5
  type: position_controllers/JointTrajectoryController
max_command_silence: 0.5
robot_state_publisher: {publish_frequency: 50.0, tf_prefix: ''}
root_frame: world
twist_controller:
  activation_buffer_ca: 50.0
  activation_buffer_jla: 300.0
  activation_threshold_ca: 0.1
  activation_threshold_jla: 10.0
  base_ratio: 0.05
  beta: 0.005
  collision_check_links: [arm_1_link, arm_2_link, arm_3_link, arm_4_link, arm_5_link,
    arm_6_link, arm_7_link]
  constraint_ca: 0
  constraint_jla: 3
  controller_interface: 0
  critical_threshold_ca: 0.025
  critical_threshold_jla: 5.0
  damping_ca: 1.0e-06
  damping_factor: 0.01
  damping_jla: 0.0001
  damping_method: 2
  enforce_pos_limits: true
  enforce_vel_limits: true
  eps_damping: 0.003
  eps_truncation: 0.001
  k_H: 1.0
  k_H_ca: -2.0
  k_H_jla: -1.0
  keep_direction: true
  kinematic_extension: 0
  lambda_max: 0.1
  max_vel_lin_base: 0.5
  max_vel_rot_base: 0.5
  mu: -2.0
  numerical_filtering: false
  priority: 500
  priority_ca: 100
  priority_jla: 50
  solver: 2
  tolerance: 5.0
  w_threshold: 0.005

@fmessmer FYI

on a fresh installation ros-indigo-joint-limits-interface is missing. This dependency is not installed using "rosdep install --from-path -i -y src"

Initial assumption:
cob4-base seems to behave "jerkier" since recently. This could be related to switching to ros_canopen, but is not guaranteed to be the reason as the behavior has not been monitored in such detail before...so no comparable plots are available.

Creating this issue to document the progress and the insights.
@ipa-mdl @ipa-fmw @ipa-tif @ipa-bnm

(Setup is latest ros_canopen (as of ros-industrial/ros_canopen@e513abc) and latest "ipa320/indigo_dev" (as of today))

Currently the +x-Axis is assumed here which is fine e.g. for torso but not correct for arm (should be +z-axis)
👉 make it configurable!

@ipa-fmw @ipa-nhg @ipa-mdl @ipa-tif

Evolving from https://github.com/ipa320/cob_common/pull/165#issuecomment-141183287 and from observations with real robot, some movements triggered through sss are still not very smooth. In particular this can be observed when:

• pressing "Stop" during a motion (e.g. arms home-side)
• trajectories with more than one trajectory point are sent (i.e. wave)
• the velocity profile at the beginning of the motion is quite "sharp"

Thus, we should:

• recheck acceleration limits for the components
• re-view joint_trajectory_controller strategy for cancle_goal()
• re-view joint_trajectory_controller interpolation strategy

@fmessmer FYI

as suggested in https://github.com/ipa320/cob_control/pull/141#issuecomment-281269449